void Motors_Init(void)
Setup IC for encoder and OC for PID control
Make sure 1 ms timer is running
Setup PWM using PWM modules
Set prescale to 24MHz/4 =6MHz
Set postscale to --> 6MHz/(4*2)=7.5kHz
Period contains 200 ticks -->PWM frequency = 7.5kHz
Set duty cycles to zero for both motors
Reset encoder counts
Assume motors are stopped, so timeout period calculation
Set motors as unlocked, so that PID control doesn't force
them to match speeds
void Motors_resetPosition(void)
Reset encoder counts
MOTOR_State_t Motors_getState(void)
If the desired speed for both motors
is zero
controlStopped is TRUE
Else
controlStopped is FALSE
If the actual speed for both motors
is zero
physicallyStopped is TRUE
Else
physicallyStopped is FALSE
If controlStopped
If physicallyStopped
return MOTORS_STOPPED;
Else return MOTORS_STOPPING;
Else return MOTORS_MOVING;
float Motors_getNetDistance(void)
Return average of the encoder counts
divided by the number of counts per inch
float Motors_getDegreesTurned(void)
Difference = Absolute Value of (Motor0 encoder
count - Motor1 encoder count)
Return 360
*Difference/2/EncoderCountsPerRevolution
long Motors_getEncoderCount(int
motorNum)
If motorNum
is zero
Return Motor0 Encoder
Count
Else
Return Motor1 Encoder
Count
void Motors_moveDiff(int
spd1, int spd0)
Note: inputs speeds = [-100, 100]
Temorarily turn off PID control using a flag
Set desired speeds equal to inputSpeed*maxSpeed
If spd1 = spd0
Set speeds as locked,
PID will keep motors at the same speed
Else
Set speeds as unlocked
Turn PID control back on
void Motors_moveStraight(int
spd)
Motors_moveDiff(spd,
spd)
void Motors_turn(MOTOR_TurnDir_t
dir, int spd)
If dir = CW
Motors_moveDiff(spd,
-1*spd);
Else
Motors_moveDiff(-1*spd, spd);
void Motors_moveStraightStop(int
spd, float inches)
Motors_moveDiff(spd,spd)
Motors_resetPosition()
Set number of encoder counts to move
equal to inches*EncoderTicksPerInch
Set flag that each motor should stop
when desired number of encoder ticks has been reached
void Motors_turnStop(MOTOR_TurnDir_t
dir, int spd, unsigned int degrees)
Motors_turn(dir, spd)
Set myEncoderTicks
equal to degrees* EncoderTicksPerRevolution/360
If dir = CCW
desEncoderCount0 = myEncoderTicks
desEncoderCount1 = -1*myEncoderTicks
Else
desEncoderCount0 = -1*myEncoderTicks;
desEncoderCount1 = myEncoderTicks;
Motors_resetPosition();
Set flag that each motor should stop
when desired number of encoder ticks has been reached
void Timers_Init(void)
Turn on timer system to allow Input
Capture
Set
pre-scale /8 = 3MHz
Set Cap/Comp 5 to Output Compare and
the rest to Input Capture
For
Input Captures on encoder signals, capture rising edges
Clear flags
Enable Interrupts
For Output Compare for PID control,
don't connect to any pin
Schedule first rise for Output
Compare for 20 [ms] in time
Clear flag
Enable Interrupts
void interrupt _Vec_tim1ch5 EncoderOC_PID_DC(void)
Note: experimentally determined
runtime for this interrupt routine to be <200[us]
Locally store variables shared
between encoder Input Capture interrupt routine and this Output Compare PID
control interrupt routine
Check newEdgeOccured
flags indicating that a new edge has occurred since the last PID loop
If no new edge has occurred
Timeout the encoders,
meaning two rising edges are required to calculate new period
Else
Reset new edge flag
Clear
interrupt flag
Program next compare to occur in 20
[ms]
Enable interrupts to ensure encoder
input capture interrupts still occur
If PID control flag has been
disabled
Return
Convert uPeriods
to speeds [RPM]
Determine Commanded Speed for Motor1
(slower motor)
First calculate speed
limit based on nearing end position
Error = desEncoderCount - EncoderCount;
spdLimit = Kp_pos*Error
newDesSpd = min(spdLimit, desSpd)
Determine Commanded Speed for Motor0
(slower motor)
If in locked mode
cmdSpd0 =
cmdSpd1;
Else If in opposite
locked mode
cmdSpd0 =
-1*cmdSpd1;
Else
Error = desEncoderCount - EncoderCount;
spdLimit = Kp_pos*Error
newDesSpd = min(spdLimit, desSpd)
Determine speed error for
PI control of speed for both motors
Calculate authority
based off PI terms and speed error
If curSpd
is zero and cmdSpd is zero
Set
authority to zero
Calculate authority
limit for both motors (based on speed->backEMF)
Limit = (CurrentMax*Res_coils +
EMF)*200[Duty Cycle Max]/16 [V]
Apply limits to
commanded authority to protect motor drivers
If at the limit
Saturate
duty cycle and prevent integrator wind-up
Update duty cycle to
implement speed control
void interrupt _Vec_tim1ch4 EncoderICRoutine(void)
Note: same code for both motors, but
located in different interrupt routines
Clear interrupt flag
If EncoderTimeout
flag
Can't make period
calculation until a second rising edge occurs
Else
Calculate elapsed time
since last rising edge
Check to make sure
period calculation is realistic, if not ignore it
Store
time of this rising edge
Read B Channel of Encoder
If B Channel is HI
Decrement encoder count
Set direction to
BACKWARD
Else
Increment encoder count
Set direction to FORWARD
If flag is set indicating motors
should stop after a given distance
If encoder count is at
that desired distance
Set desired
speed of motors to zero
Reset flag
indicating motors should stop after given distance
static void Motor0(int inputDC)
Note: inputDC
= [-200, 200]
Negate
input duty cycle command because motor 0 needs to spin "backwards"
for the robot to go forwards
If
inputDC > 0
Set one side of coil
driver low
Set PWM duty on other
side of coil to input duty cycle
Else If inputDC
< 0
Set one side of coil
driver high
Set PWM duty on other
side of coil to 200 + inputDC
Else
Set one side of coil driverlow
Set PWM duty on other
side of coil to zero
static void Motor1(int inputDC)
Note: inputDC
= [-200, 200]
If
inputDC > 0
Set one side of coil
driver low
Set PWM duty on other
side of coil to input duty cycle
Else If inputDC
< 0
Set one side of coil
driver high
Set PWM duty on other
side of coil to 200 + inputDC
Else
Set one side of coil driverlow
Set PWM duty on other
side of coil to zero