Beruflich Dokumente
Kultur Dokumente
/****************************************************************************
Module
DCMotor.c
Revision
1.0.1
Description
This is a template file for implementing a simple service under the
Gen2 Events and Services Framework.
Notes
History
When Who
--------------------
****************************************************************************/
//Position control
void InitMotors(void){
//init hardware for PWM channels, IN1 pins, motor encoders, oneshot timers and
periodic timer
InitPWM();
InitIN1Pins();
InitLeftEncoderCapture();
InitRightEncoderCapture();
InitPeriod2MSTimer();
InitLeftOneShotTimer();
InitRightOneShotTimer();
#ifdef DCMOTOR_TEST
RobotX = 0.0; RobotY = 0.0;
#endif
SetRobotDir(Forward);
}
void StopRobot(void){
//set duty cycle to both motors 0
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
HWREG( PWM0_BASE+PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ZERO;
// kill the closed loop speed updating timer
HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
void LeftCaptureResponse(void){
uint32_t ThisCapture;
uint32_t ThisPeriod;
uint8_t Divisor;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_CAECINT;
// ES_Event_t EventToPost;
// EventToPost.EventType = DEBUG_ENCODER;
// EventToPost.EventParam = ThisCapture;
// PostGameMasterSM(EventToPost);
HWREG(WTIMER4_BASE+TIMER_O_TAV) = HWREG(WTIMER4_BASE+TIMER_O_TAILR);
// now kick the one-shot timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
// isLeftStop = false;
}
void RightCaptureResponse(void){
uint32_t ThisCapture;
uint32_t ThisPeriod;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_CBECINT;
if (RobotDirection == Backward)
{
RobotX -= PulseDistance * cos(theta);
RobotY -= PulseDistance * sin(theta);
}
else if (RobotDirection == Forward)
{
RobotX += PulseDistance * cos(theta);
RobotY += PulseDistance * sin(theta);
}
else if (RobotDirection == CWTurn)
{
theta -= PulseDistance * 0.9/ TurnRadius;
}
else if (RobotDirection == CCWTurn)
{
theta += PulseDistance * 0.9 / TurnRadius;
}
int XError;
int YError;
float HeadingError;
void SpeedUpdateISR(void){
// static uint16_t LeftRequestedDuty;
// static uint16_t RightRequestedDuty;
// start by clearing the source of the interrupt
HWREG(WTIMER3_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;
//void PositionCheckISR(void)
//{
// // start by clearing the source of the interrupt
// HWREG(WTIMER3_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
// int XError;
// int YError;
// int HeadingError;
//
// //Check to see if target position or Heading has been reached
// if (RobotDirection == Backward || RobotDirection == Forward)
// {
// XError = (int)RobotX - TargetX;
// YError = (int)RobotY - TargetY;
// if ((abs(XError) < 10) && (abs(YError) < 10))
// {
// ES_Event_t ReturnEvent;
// ReturnEvent.EventType = EV_TARGET_REACHED;
// PostGameMasterSM(ReturnEvent);
// HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// }
// }
// else
// {
// HeadingError = (int)(theta*180/pi) - TargetTheta;
// if (abs(HeadingError) < 3)
// {
// ES_Event_t ReturnEvent;
// ReturnEvent.EventType = EV_TARGET_REACHED;
// PostGameMasterSM(ReturnEvent);
// HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// }
// }
//}
void LeftOneShotTimerISR(void){
// start by clearing the source of the interrupt
HWREG(WTIMER4_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;
void RightOneShotTimerISR(void){
// start by clearing the source of the interrupt
HWREG(WTIMER4_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
//if we just changed right motor direction
if (JustChangedDirection == true)
{
//set RightCurrentRPM to be 0
RightCurrentRPM = 0;
//set JustChangedDirection to be false
JustChangedDirection = false;
}else{ //else post EV_MOTOR_STOP to GameMasterSM
ES_Event_t Event;
Event.EventType = EV_MOTOR_STOP;
PostGameMasterSM(Event);
}
}
void printRPM(void){
//the origianl calculation is float LRPM =
(SecInMin*TicksPerSec)/(PulsePerRev*LPeriod)/GearRatio;
//to convert to float arithmetic and prevent overflow, I calculate
SecInMin/GearRatio = 1.2 first
printf("LPeriod = %d, RPeriod = %d \n\r", LPeriod, RPeriod);
printf("LeftRequestedDuty = %d, LeftTargetRPM = %d, LeftCurrentRPM = %d \n\r",
LeftRequestedDuty, LeftTargetRPM, LeftCurrentRPM);
printf("RightRequestedDuty = %d, RightTargetRPM = %d, RightCurrentRPM = %d \n\r",
RightRequestedDuty, RightTargetRPM, RightCurrentRPM);
printf("CurrentXPosition = %.2f, CurrentYPosition = %.2f, CurrentAngle = %.2f
\n\r", RobotX, RobotY, theta);
printf("EncoderPulses = %d \n\r", PulseCounts);
printf("targettheta = %d, theta = %.2f\n\r\n\r", TargetTheta,
(int)theta*180/3.1415);
}
/******************************************private
functions********************************************/
static void SetDutyCycle(uint8_t which, uint32_t PWM){
if(which == LeftMotor){ //if we are setting LeftMotor PWM
if (PWM <= 0){ //if required PWM is equal or less than 0
//set PWM to 0
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
} else if (PWM >= 100) { //else if required PWM is equal or greater than 100
//set PWM to 100
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ONE;
} else { //else
//set PWM to proper value
HWREG( PWM0_BASE+PWM_O_0_GENA) = GenA_Normal;
HWREG( PWM0_BASE+PWM_O_0_CMPA) = (HWREG(PWM0_BASE+PWM_O_0_LOAD)) * (100 -
PWM)/100;
}
}
// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0)
;
// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period. We will also program
// the match compare registers to 1/2 the desired high time
HWREG( PWM0_BASE+PWM_O_0_LOAD) = ((PeriodInMS * PWMTicksPerMS))>>1; // Period
// Set the initial Duty cycle on A to 50% by programming the compare value
// to 1/2 the period to count up (or down). Technically, the value to program
// should be Period/2 - DesiredHighTime/2, but since the desired high time is
1/2
// the period, we can skip the subtract
HWREG( PWM0_BASE+PWM_O_0_CMPA) = HWREG( PWM0_BASE+PWM_O_0_LOAD)>>1; // Left
Motor Duty cycle 50%
// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bits 6 and 7
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) =
(HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0x00ffffff) +
(4<<(7*BitsPerNibble)) +
(4<<(6*BitsPerNibble));
// set the up/down count mode, enable the PWM generator and make
// both generator updates locally synchronized to zero count
HWREG(PWM0_BASE+ PWM_O_0_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE |
PWM_0_CTL_GENAUPD_LS |
PWM_0_CTL_GENBUPD_LS);
}
/******************************************************Encoders*****************
*********************************************/
//Left encoder is PD0 (WT2CCP0)
static void InitLeftEncoderCapture(void){
// start by enabling the clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// enable the clock to Port D
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R3;
// since we added this Port D clock init, we can immediately start
// into configuring the timer, no need for further delay
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// we want to use the full 32 bit count, so initialize the Interval Load
// register to 0xffffffff (its default value)
HWREG(WTIMER2_BASE+TIMER_O_TAILR) = 0xffffffff;
// set up timer A in capture mode (TAMR=3, TAAMS = 0),
// for edge time (TACMR = 1) and up-counting (TACDIR = 1)
HWREG(WTIMER2_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER2_BASE+TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) |
(TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
// To set the event to rising edge, we need to modify the TAEVENT bits
// in GPTMCTL. Rising edges = 00, so we set the TAEVENT bits
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
// Now Set up the port to do the capture (clock was enabled earlier)
// start by setting the alternate function for Port D bit 0 (WT2CCP0)
HWREG(GPIO_PORTD_BASE+GPIO_O_AFSEL) |= BIT0HI;
// Then, map bit 4's alternate function to WT0CCP0
// 7 is the mux value to select WT0CCP0, 0 to shift it over to the
// right nibble for bit 0 (4 bits/nibble * 0 bits)
HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) &
0xfffffff0) + (7<<0);
// Enable pin on Port D for digital I/O
HWREG(GPIO_PORTD_BASE+GPIO_O_DEN) |= BIT0HI;
// make pin 0 on Port D into an input
HWREG(GPIO_PORTD_BASE+GPIO_O_DIR) &= BIT0LO;
// back to the timer to enable a local capture interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
// enable the Timer A in Wide Timer 2 interrupt in the NVIC
// it is interrupt number 98 so appears in EN2 at bit 30
HWREG(NVIC_EN3) |= BIT2HI;
// make sure interrupts are enabled globally
__enable_irq();
// Start the input capture timer later when needed
// now kick the InputCaptureTimer off by enabling it and
//enabling the timer to stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}
// lower priority
//HWREG(NVIC_PRI24) = (HWREG(NVIC_PRI24) & ~NVIC_PRI24_INTC_M) + (1<<21);
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger.
HWREG(WTIMER3_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger.
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger.
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}