#define TEST

/**************************************************************************

Motors.c

ME 218B Project Hats Off

Team Firefox

March 7, 2010

The robot uses 2 DC motors. Implements PWM

Versions:

2.1 02-24-10 Lock wheel speeds, smarter duty cycle limitation (pos and neg)

2.0 02-21-10 Acceleration limits, position control, min controllable speed

1.1 02-19-10 Motors stop at same time when one reaches desired number of encoder counts.

1.0 02-17-10 PID control works but commanding zero speed as each motor

reaches target position doesn't work (robot spins wildly

near end of maneuver)

***************************************************************************/

/*-------------------------- Include Files -------------------------------*/

#include "Motors.h"

typedef enum { FORWARD,

BACKWARD

} DIR_t;

typedef enum { LOCKED,

OPPOSITE,

UNLOCKED

} LOCKMODE_t;

/*-------------------------- Module Definitions -----------------------------*/

//#defines pins for portability

#define DCMOTOR0 BIT1HI //PP1

#define DCMOTOR1 BIT3HI //PP3

#define POTPIN 7 //PTAD07

#define _MS_ *3000 //=3MHz = (24MHz / 8) from Timer prescale

//Takes ~21[ms] before overflow

#define TIME (20 _MS_) //Number of seconds I want to wait during OC

#define printf (void)printf

/*-------------------------- Module Functions -----------------------------*/

static void Motor0(int DC); //command duty cycle [-100, 100]

static void Motor1(int DC); //command duty cycle [-100, 100]

static void Timers_Init(void); //initializes input capture for encoder readings

//and output compare for PID control

/*-------------------------- Module Variables -----------------------------*/

static unsigned int uPeriod0, uPeriod1; //period of last encoder tick for each motor 1.34 [us]

static DIR_t dir0, dir1; //encoder direction

static char newEdge0, newEdge1; //indicates if an encoder pulse has occured in last 87 [ms]

static int desSpd0, desSpd1; //desired speeds of motors 0 and 1

static int curSpd0, curSpd1; //current speeds of motors 0 and 1 (calculated every 20 [ms]

static int duty0, duty1; //current duty cycles of motors

static LOCKMODE_t curLockMode;

static long EncoderCount0, EncoderCount1; //encoder tick count

static char EncoderTimeout0, EncoderTimeout1; //used to time out encoder pulses when motor is stopped

static long desEncoderCount0 = 0, desEncoderCount1 = 0; //desired number of encoder steps

static char desEncoderFlag0 = 0, desEncoderFlag1 = 0; //indicates that motors should stop after reaching desEncoderCount

static unsigned int disablePID = 0; //disables speed control

/*-------------------------- Module Cal Variables -------------------------*/

const static int minControlRPM = 40; //Minimum speed at which PID can control (actual value based on PID loop time

//is 27.4 RPM

const static float Kp = 0.024, Ki = 0.006; //speed control Kp = 0.012, Ki = 0.004

const static float Kp_Pos = 2; //1.6//0.6; //position control: CmdVelocity = Kp_pos*ErrorPosition [Encoder Counts]

const static int maxPosAccel = 250, maxNegAccel = 1000; //70;//max accel rate in [RPM/Control Loop Time] = [RPM/20ms]

const static int DutyOffset = 8; //add 2 extra duty points to motor 1 cuz he's slow

//const static

// linear distance cals

const static int EncoderTicksPerInch = 288; //assumes GR = 1.33 and wheel diameter = 110 [mm]

const static int EncoderTicksPerRev = 7329;// //7420 //7343; //assumes distance between wheels = 8 [in] and all the

above

const static int maxMotorSpeed = 4500; //calc: 5830 // [rpm] of motor = 53*output shaft speed @14V

//debug variables

static unsigned int lastPrint = 0;

static unsigned int minUPeriod = 130;

static unsigned int maxUPeriod = 21000;

void Motors_Init(void) {

Timers_Init(); //setup IC for encoder and OC for PID control

MyTimer_Init(); //Make sure 1 ms timer is running

lastPrint = MyTimer_GetTime();

//setup PWM using PWM modules

PWM0_Init(1,1);

PWMPRCLK = (PWMPRCLK &~ _S12_PCKA2 &~ _S12_PCKA0) | _S12_PCKA1; //set prescale to 24MHz/4 =6MHz

PWMSCLA = 2; //set postscale to --> 6MHz/(4*2)=7.5kHz

setPWM0Period(200); //period contains 100 ticks -->PWM frequency = 15kHz

PWM1_Init(1,1);

PWMPRCLK = (PWMPRCLK &~ _S12_PCKA2 &~ _S12_PCKA0) | _S12_PCKA1; //set prescale to 24MHz/4 =6MHz

PWMSCLA = 2; //set postscale to --> 6MHz/(4*2)=7.5kHz

setPWM1Period(200); //period contains 100 ticks -->PWM frequency = 15kHz

Motor0(0);

Motor1(0);

//reset encoder counts

EncoderCount0 = 0;

EncoderCount1 = 0;

//assume motors are stopped, so timeout period calculation

EncoderTimeout0 = 1;

EncoderTimeout1 = 1;

//set motors as unlocked

curLockMode = UNLOCKED;

}

//use only for debugging, will invalidate getState() function if callled

void Motors_debugMove(int duty1, int duty0) {

if (duty0 != 0 || duty1 != 0) {

disablePID = 1;

Motor0(duty0);

Motor1(duty1);

}

else

disablePID = 0;

}

void Motors_debugPrint(char motorNum) {

//print debugging information to the screen

DIR_t myDir0, myDir1;

char myNewEdge0, myNewEdge1;

unsigned int myUPeriod0, myUPeriod1;

int myDuty0, myDuty1;

DisableInterrupts;

//retrieve shared variables between encoder IC interrupt and this print statement

myDir0 = dir0;

myDir1 = dir1;

myNewEdge0 = newEdge0;

myNewEdge1 = newEdge1;

myUPeriod0 = uPeriod0;

myUPeriod1 = uPeriod1;

myDuty0 = duty0;

myDuty1 = duty1;

EnableInterrupts;

if (motorNum == 0)

(void)printf("curRPM0:%d, desRPM0:%d, duty0:%d, period0:%d\r\n",curSpd0, desSpd0, duty0, uPeriod0);

else

(void)printf("curRPM1:%d, desRPM1:%d, duty1:%d, period1:%d\r\n",curSpd1, desSpd1, duty1, uPeriod1);

}

void Motors_resetPosition(void) {

//reset encoder counts

EncoderCount0 = 0;

EncoderCount1 = 0;

}

MOTOR_State_t Motors_getState(void) {

char physicallyStopped;

char controlStopped;

controlStopped = ((desSpd0 == 0) && (desSpd1 == 0));

physicallyStopped = ((curSpd0 == 0) && (curSpd1 == 0));

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 since last command

return (((float)EncoderCount0 + (float)EncoderCount1)/(2*EncoderTicksPerInch));

}

float Motors_getDegreesTurned(void) {

float temp;

temp = (360*((float)EncoderCount0 - (float)EncoderCount1)/(2*EncoderTicksPerRev));

if (temp < 0) temp *= -1;

return temp;

}

long Motors_getEncoderCount(int motorNum) {

if (motorNum == 0) return EncoderCount0;

else return EncoderCount1;

}

//assumes inputs speeds = [-100, 100]

void Motors_moveDiff(int spd1, int spd0) {

disablePID = 1; //temporarily turn off PID control

if (spd0 == 0) desSpd0 = 0;

else {

desSpd0 = (int)((long)spd0*maxMotorSpeed/100);

//make sure commanded speed isn't slower than min controllable speed

if (desSpd0 > 0 && desSpd0 < minControlRPM) desSpd0 = minControlRPM;

if (desSpd0 < 0 && desSpd0 > -1*minControlRPM) desSpd0 = -1*minControlRPM;

}

if (spd1 == 0) desSpd1 = 0;

else {

desSpd1 = (int)((long)spd1*maxMotorSpeed/100);

//make sure commanded speed isn't slower than min controllable speed

if (desSpd1 > 0 && desSpd1 < minControlRPM) desSpd1 = minControlRPM;

if (desSpd1 < 0 && desSpd1 > -1*minControlRPM) desSpd1 = -1*minControlRPM;

}

desEncoderFlag0 = 0;

desEncoderFlag1 = 0;

if (spd1 == spd0) curLockMode = LOCKED;

else {

if (spd1 == -1*spd0) curLockMode = OPPOSITE;

else curLockMode = UNLOCKED;

}

disablePID = 0; //reengage PID control

}

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) {

float myDistance = 0;

if (spd < 0) myDistance = -1*inches;

else myDistance = inches;

Motors_moveDiff(spd,spd); //set motor speeds

Motors_resetPosition(); //reset encoder counts

//set number of encoder counts to move

desEncoderCount0 = (int)(myDistance*EncoderTicksPerInch);

desEncoderCount1 = (int)(myDistance*EncoderTicksPerInch);

desEncoderFlag0 = 1; //indicate wheel should stop after given # of encoder counts

desEncoderFlag1 = 1;

}

void Motors_turnStop(MOTOR_TurnDir_t dir, int spd, unsigned int degree) {

long myEncoderTicks = 0;

Motors_turn(dir, spd); //set motor speeds

//set number of encoder counts to move

myEncoderTicks = ((long)degree)*EncoderTicksPerRev/360;

if (dir == CCW) {

desEncoderCount0 = myEncoderTicks;

desEncoderCount1 = -1*myEncoderTicks;

}

else {

desEncoderCount0 = -1*myEncoderTicks;

desEncoderCount1 = myEncoderTicks;

}

Motors_resetPosition();

desEncoderFlag0 = 1; //indicate wheel should stop after given # of encoder counts

desEncoderFlag1 = 1;

}

//Initialize interrupts

//Encoder0 Channel A is Timer 1, PT4

//Encoder1 Channel A is Timer 1, PT6

//OC for PID control is Timer 1 PT5

void Timers_Init(void){

TIM1_TSCR1 = _S12_TEN; //Turn on timer system to allow IC

TIM1_TSCR2 = (TIM1_TSCR2 & ~_S12_PR2) |_S12_PR1 | _S12_PR0; //pre-scale /8 = 3MHz

TIM1_TIOS = _S12_IOS5; //set Cap/Comp 5 to OC. Rest are IC.

/** InputCapture with interrupt of Encoder0 Channel A.

Use Timer1 Channel4 set to PT4 on E128 **/

TIM1_TCTL3 = (TIM1_TCTL3 &~ _S12_EDG4B) | _S12_EDG4A; //capture rising edges

TIM1_TFLG1 = _S12_C4F; //Clear IC4 flag

TIM1_TIE |= _S12_C4I; //Enable IC4 interrupt

/** InputCapture with interrupt of Encoder1 Channel A.

Use Timer1 Channel6 set to PT6 on E128 **/

TIM1_TCTL3 = (TIM1_TCTL3 &~ _S12_EDG6B) | _S12_EDG6A; //capture rising edges

TIM1_TFLG1 = _S12_C6F; //Clear IC4 flag

TIM1_TIE |= _S12_C6I; //Enable IC4 interrupt

/** OutputCompare with interrupt on

Use Timer1 Channel5 set to PT5 on E128 **/

TIM1_TCTL1 = TIM1_TCTL1 & ~(_S12_OL5 | _S12_OM5); //No pin connected for OC5

TIM1_TC5 = TIM1_TCNT + TIME; //Schedule first rise for OC

TIM1_TFLG1 = _S12_C5F; //Clear OC5 flag

TIM1_TIE |= _S12_C5I; //Enable OC5 interrupt

EnableInterrupts;

}

//PID Control Interrupt Response Routine is called every 20 [ms]

//Measured runtime < 200 [us]

void interrupt _Vec_tim1ch5 EncoderOC_PID_DC(void){

static long intErr0=0, intErr1=0; //integrated errors

static int lastCmdSpd0=0, lastCmdSpd1=0; //last commanded speed for acceleration control

int errSpd0, errSpd1;

long errPos0, errPos1;

float tempFloat;

int curMaxDuty0, curMinDuty0, curMaxDuty1, curMinDuty1;

int cmdSpd0, cmdSpd1; //these are same as desired speed BUT with acceleration and position

//control constraints imposed

DIR_t myDir0, myDir1;

char myNewEdge0, myNewEdge1;

unsigned int myUPeriod0, myUPeriod1;

//PTT |= BIT7HI; //debugging

//retrieve shared variables between encoder IC interrupt and this OC control interrupt

myDir0 = dir0;

myDir1 = dir1;

myNewEdge0 = newEdge0;

myNewEdge1 = newEdge1;

myUPeriod0 = uPeriod0;

myUPeriod1 = uPeriod1;

//write shared variables

newEdge0 = 0;

newEdge1 = 0;

if (myNewEdge0 == 0) EncoderTimeout0 = 1;

if (myNewEdge1 == 0) EncoderTimeout1 = 1;

TIM1_TFLG1 = _S12_C5F; //Clear OC5 flag

TIM1_TC5 = TIM1_TCNT + TIME; //Program next compare

EnableInterrupts; //Call to ensure Encoder IC interrupts

//check if commanding duty cycle directly for debugging purposes

if (disablePID == 1) {

//do not implement PID control

return;

}

//convert uPeriods to speeds [RPM]

if (myNewEdge0 == 0) curSpd0 = 0;

else {

curSpd0 = (int)((long)1800000/myUPeriod0); // motor speed

if (myDir0 == BACKWARD) curSpd0 *= -1;

}

if (myNewEdge1 == 0) curSpd1 = 0;

else {

curSpd1 = (int)((long)1800000/myUPeriod1); // motor speed

if (myDir1== BACKWARD) curSpd1 *= -1;

}

cmdSpd0 = desSpd0;

cmdSpd1 = desSpd1;

//Determine Commanded Speed for Motor1 (slower motor)

//Calc speed limit based on nearing end position

if (desEncoderFlag1 == 1) {

errPos1 = desEncoderCount1 - EncoderCount1;

//calculate cmd speed based off this error

if (errPos1 < 0) cmdSpd1 = (int)(Kp_Pos*errPos1) - minControlRPM;

else cmdSpd1 = (int)(Kp_Pos*errPos1) + minControlRPM;

if (desSpd1 >= 0) {

if((cmdSpd1 > desSpd1) || (cmdSpd1 < 0)) cmdSpd1 = desSpd1;

}

else {

if((cmdSpd1 < desSpd1) || (cmdSpd1 > 0)) cmdSpd1 = desSpd1;

}

}

//Calc speed limit based on acceleration limits

//if ((cmdSpd1 - lastCmdSpd1) > maxPosAccel) cmdSpd1 = lastCmdSpd1 + maxPosAccel;

//else if ((cmdSpd1 - lastCmdSpd1) < -1*maxNegAccel) cmdSpd1 = lastCmdSpd1 - maxNegAccel;

//Determine Commanded Speed for Motor0 (faster motor)

if (curLockMode == LOCKED) cmdSpd0 = cmdSpd1;

else if (curLockMode == OPPOSITE) cmdSpd0 = -1*cmdSpd1;

else {

//Calc speed limit based on nearing end position

if (desEncoderFlag0 == 1) {

//calculate position error

//this error should NEVER be zero (or we wouldn't be in position control mode)

errPos0 = desEncoderCount0 - EncoderCount0;

//calculate cmd speed based off this error

if (errPos0 < 0) cmdSpd0 = (int)(Kp_Pos*errPos0) - minControlRPM;

else cmdSpd0 = (int)(Kp_Pos*errPos0) + minControlRPM;

//cmdSpd from position control should only limit desSpd, never increase it

if (desSpd0 >= 0) {

if((cmdSpd0 > desSpd0) || (cmdSpd0 < 0)) cmdSpd0 = desSpd0;

}

else {

if((cmdSpd0 < desSpd0) || (cmdSpd0 > 0)) cmdSpd0 = desSpd0;

}

}

//Calc speed limit based on acceleration limits

//if ((cmdSpd0 - lastCmdSpd0) > maxPosAccel) cmdSpd0 = lastCmdSpd0 + maxPosAccel;

//else if ((cmdSpd0 - lastCmdSpd0) < -1*maxNegAccel) cmdSpd0 = lastCmdSpd0 - maxNegAccel;

}

//update for last cmd speed for acceration limit purposes

lastCmdSpd0 = cmdSpd0;

lastCmdSpd1 = cmdSpd1;

//determine speed error for PID control of speed

errSpd0 = cmdSpd0 - curSpd0;

errSpd1 = cmdSpd1 - curSpd1;

intErr0 += errSpd0;

intErr1 += errSpd1;

//calculate authority

tempFloat = Kp*errSpd0 + Ki*intErr0;

duty0 = (int)tempFloat;

tempFloat = Kp*errSpd1 + Ki*intErr1;

duty1 = (int)tempFloat;

//quick check for zero duty

if ((curSpd0 == 0) && (cmdSpd0 == 0)) duty0 = 0;

if ((curSpd1 == 0) && (cmdSpd1 == 0)) duty1 = 0;

//Calculate authority limit for both motors (based on speed->backEMF)

tempFloat = curSpd0/407; //calculate back EMF

curMaxDuty0 = (int)( 157 + 12.5*tempFloat); //(( Imax*Rcoils) + EMF)*200[Duty Cycle]/16[V];

curMinDuty0 = (int)(-157 + 12.5*tempFloat); //((-Imax*Rcoils) + EMF)*200/16;

if (curMaxDuty0 > 200) curMaxDuty0 = 200;

if (curMinDuty0 < -200) curMinDuty0 = -200;

tempFloat = curSpd1/407; //calculate back EMF

curMaxDuty1 = (int)( 157 + 12.5*tempFloat); //(( Imax*Rcoils) + EMF)*200/16;

curMinDuty1 = (int)(-157 + 12.5*tempFloat); //((-Imax*Rcoils) + EMF)*200/16;

if (curMaxDuty1 > 200) curMaxDuty1 = 200;

if (curMinDuty1 < -200) curMinDuty1 = -200;

//saturate duty cycle and prevent integrator wind-up

if (duty0 < curMinDuty0) {

duty0 = curMinDuty0;

intErr0 -= errSpd0;

}

else {

if (duty0 > curMaxDuty0) {

duty0 = curMaxDuty0;

intErr0 -= errSpd0;

}

}

if (duty1 < curMinDuty1) {

duty1 = curMinDuty1;

intErr1 -= errSpd1;

}

else {

if (duty1 > curMaxDuty1) {

duty1 = curMaxDuty1;

intErr1 -= errSpd1;

}

}

//update duty cycle to implement speed control

Motor0(duty0);

Motor1(duty1);

//PTT &= BIT7LO;

}

//Encoder0 Channel A tick interrupt for Motor 0

//Interrupt Response Routine is called when TFLG1bit4 is set

//meaning interrupt of rising edge has occurred

void interrupt _Vec_tim1ch4 Encoder0ICRoutine(void){

static unsigned int lastEdge0, tempPeriodCalc;

static int i = 0;

TIM1_TFLG1 = _S12_C4F; //Clear IC4 flag

if (EncoderTimeout0 == 1) {

EncoderTimeout0 = 0; //can't make period calculation until a second pulse is received

}

else {

tempPeriodCalc = TIM1_TC4 - lastEdge0;

//check to make sure period calculation is realistic

if ((tempPeriodCalc < minUPeriod) || (tempPeriodCalc > maxUPeriod)) {

//ignore this edge, must be noise

return;

}

uPeriod0 = tempPeriodCalc;

newEdge0 = 1; //flag that an encoder tick has occured

}

//capture last edge timing

lastEdge0 = TIM1_TC4;

if (PTP & BIT4HI) {

EncoderCount0--;

dir0 = BACKWARD;

//printf("BACKWARD0\r\n"); //remove

}

else {

EncoderCount0++;

dir0 = FORWARD;

}

//check if motor should stop MOTORS_MOVING

if (desEncoderFlag0 == 1) {

if (desEncoderCount0 == EncoderCount0) {

desSpd0 = 0; //stop the motor

desEncoderFlag0 = 0;

if (curLockMode != UNLOCKED) {

desSpd1 = 0;

desEncoderFlag1 = 0;

}

}

}

}

//Encoder tick interrupt for Motor 1

//Interrupt Response Routine is called when TFLG1bit6 is set

//meaning interrupt of rising edge has occurred

void interrupt _Vec_tim1ch6 Encoder1ICRoutine(void){

static int i = 0;

static unsigned int lastEdge1, tempPeriodCalc;

TIM1_TFLG1 = _S12_C6F; //Clear IC5 flag

if (EncoderTimeout1 == 1) {

EncoderTimeout1 = 0; //can't make period calculation until a second pulse is received

}

else {

tempPeriodCalc = TIM1_TC6 - lastEdge1;

if ((tempPeriodCalc < minUPeriod) || (tempPeriodCalc > maxUPeriod)) {

//ignore this edge, must be noise

return;

}

uPeriod1 = tempPeriodCalc;

newEdge1 = 1; //flag that an encoder tick has occured

}

lastEdge1 = TIM1_TC6;

if (PTP & BIT5HI) {

EncoderCount1++;

dir1 = FORWARD;

}

else {

EncoderCount1--;

dir1 = BACKWARD;

}

//check if motor should stop MOTORS_MOVING

if (desEncoderFlag1 == 1) {

if (desEncoderCount1 == EncoderCount1) {

desSpd1 = 0; //stop the motor

desEncoderFlag1 = 0;

if (curLockMode != UNLOCKED) {

desSpd0 = 0;

desEncoderFlag0 = 0;

}

}

}

}

//set duty cycle on motor 0 = where duty = [0, 200]

static void Motor0(int inputDC){

int DC = -1*inputDC; // motor 1 needs to spin "backwards" for the bot to go forward

if(DC > 0){ //CW

PTP &= ~DCMOTOR0; //PTT = PTT & BIT0LO

setPWM0DutyCycle(DC);

}

else if (DC < 0){ //CCW

PTP |= DCMOTOR0;

setPWM0DutyCycle(200+DC);

}

else { //stop

PTP &= ~DCMOTOR0;

setPWM0DutyCycle(DC);

}

}

//set duty cycle on motor 1

static void Motor1(int DC){

//motor 1 is slower than motor 0

if(DC > 0){

DC+=DutyOffset;

if (DC > 200) DC = 200;

PTP &= ~DCMOTOR1;

setPWM1DutyCycle(DC);

}

else if (DC < 0){

DC-=DutyOffset;

if (DC < -200) DC = -200;

PTP |= DCMOTOR1;

setPWM1DutyCycle(200+DC);

}

else {

PTP &= ~DCMOTOR1;

setPWM1DutyCycle(DC);

}

}