Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_AutonomousDriver

Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.

Dependencies:   IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos

main.cpp@0:84d5aa80fd77, 2014-04-27 (annotated)

Committer:
Strikewolf
Date:
Sun Apr 27 04:31:07 2014 +0000
Revision:
0:84d5aa80fd77
-v1.0 of human controlled car;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Strikewolf 0:84d5aa80fd77 1 #include "mbed.h"
Strikewolf 0:84d5aa80fd77 2
Strikewolf 0:84d5aa80fd77 3 //Radio Calibration Signals - Need to implement calibration routine...
Strikewolf 0:84d5aa80fd77 4 #define CHAN2_MID 1470 //Elevator Midpoint
Strikewolf 0:84d5aa80fd77 5 #define CHAN1_MID 1470 //Rudder Midpoint
Strikewolf 0:84d5aa80fd77 6 #define DEADZONE 105
Strikewolf 0:84d5aa80fd77 7 #define MAXMIN_OFFSET 400
Strikewolf 0:84d5aa80fd77 8
Strikewolf 0:84d5aa80fd77 9 //Drive Modes
Strikewolf 0:84d5aa80fd77 10 typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
Strikewolf 0:84d5aa80fd77 11 DRIVEMODE mode;
Strikewolf 0:84d5aa80fd77 12 float xpwm, ypwm;
Strikewolf 0:84d5aa80fd77 13
Strikewolf 0:84d5aa80fd77 14 //H-bridge
Strikewolf 0:84d5aa80fd77 15 PwmOut rightMotorPWM(p22); //Channel A
Strikewolf 0:84d5aa80fd77 16 PwmOut leftMotorPWM(p21); //Channel B
Strikewolf 0:84d5aa80fd77 17 DigitalOut leftMotor1(p23);
Strikewolf 0:84d5aa80fd77 18 DigitalOut leftMotor2(p24);
Strikewolf 0:84d5aa80fd77 19 DigitalOut rightMotor1(p25);
Strikewolf 0:84d5aa80fd77 20 DigitalOut rightMotor2(p26);
Strikewolf 0:84d5aa80fd77 21
Strikewolf 0:84d5aa80fd77 22 //Radio
Strikewolf 0:84d5aa80fd77 23 Serial pc(USBTX, USBRX);
Strikewolf 0:84d5aa80fd77 24 DigitalIn rudder(p11);
Strikewolf 0:84d5aa80fd77 25 DigitalIn elevator(p12);
Strikewolf 0:84d5aa80fd77 26
Strikewolf 0:84d5aa80fd77 27 //Xbee
Strikewolf 0:84d5aa80fd77 28 Serial xbee(p13, p14);
Strikewolf 0:84d5aa80fd77 29 DigitalOut xbeeReset(p15);
Strikewolf 0:84d5aa80fd77 30
Strikewolf 0:84d5aa80fd77 31 void setTurnLeft(float speed) {
Strikewolf 0:84d5aa80fd77 32 //Set speed
Strikewolf 0:84d5aa80fd77 33 rightMotorPWM = speed;
Strikewolf 0:84d5aa80fd77 34 leftMotorPWM = speed;
Strikewolf 0:84d5aa80fd77 35
Strikewolf 0:84d5aa80fd77 36 //Set motor function
Strikewolf 0:84d5aa80fd77 37 leftMotor1 = 0;
Strikewolf 0:84d5aa80fd77 38 leftMotor2 = 1;
Strikewolf 0:84d5aa80fd77 39 rightMotor1 = 1;
Strikewolf 0:84d5aa80fd77 40 rightMotor2 = 0;
Strikewolf 0:84d5aa80fd77 41 }
Strikewolf 0:84d5aa80fd77 42
Strikewolf 0:84d5aa80fd77 43 void setTurnRight(float speed) {
Strikewolf 0:84d5aa80fd77 44 //Set speed
Strikewolf 0:84d5aa80fd77 45 rightMotorPWM = speed;
Strikewolf 0:84d5aa80fd77 46 leftMotorPWM = speed;
Strikewolf 0:84d5aa80fd77 47
Strikewolf 0:84d5aa80fd77 48 //Set motor function
Strikewolf 0:84d5aa80fd77 49 leftMotor1 = 1;
Strikewolf 0:84d5aa80fd77 50 leftMotor2 = 0;
Strikewolf 0:84d5aa80fd77 51 rightMotor1 = 0;
Strikewolf 0:84d5aa80fd77 52 rightMotor2 = 1;
Strikewolf 0:84d5aa80fd77 53 }
Strikewolf 0:84d5aa80fd77 54
Strikewolf 0:84d5aa80fd77 55 //Stop with braking
Strikewolf 0:84d5aa80fd77 56 void stop() {
Strikewolf 0:84d5aa80fd77 57 rightMotorPWM = 0;
Strikewolf 0:84d5aa80fd77 58 leftMotorPWM = 0;
Strikewolf 0:84d5aa80fd77 59 leftMotor1 = 0;
Strikewolf 0:84d5aa80fd77 60 leftMotor2 = 1;
Strikewolf 0:84d5aa80fd77 61 rightMotor1 = 1;
Strikewolf 0:84d5aa80fd77 62 rightMotor2 = 0;
Strikewolf 0:84d5aa80fd77 63 }
Strikewolf 0:84d5aa80fd77 64
Strikewolf 0:84d5aa80fd77 65
Strikewolf 0:84d5aa80fd77 66 int getRadioX() {
Strikewolf 0:84d5aa80fd77 67 Timer timer;
Strikewolf 0:84d5aa80fd77 68 timer.reset();
Strikewolf 0:84d5aa80fd77 69 int dur;
Strikewolf 0:84d5aa80fd77 70 while(!rudder);
Strikewolf 0:84d5aa80fd77 71 timer.start();
Strikewolf 0:84d5aa80fd77 72 while(rudder);
Strikewolf 0:84d5aa80fd77 73 dur = timer.read_us();
Strikewolf 0:84d5aa80fd77 74 return dur;
Strikewolf 0:84d5aa80fd77 75 }
Strikewolf 0:84d5aa80fd77 76
Strikewolf 0:84d5aa80fd77 77 int getRadioY() {
Strikewolf 0:84d5aa80fd77 78 Timer timer;
Strikewolf 0:84d5aa80fd77 79 timer.reset();
Strikewolf 0:84d5aa80fd77 80 int dur;
Strikewolf 0:84d5aa80fd77 81 while(!elevator);
Strikewolf 0:84d5aa80fd77 82 timer.start();
Strikewolf 0:84d5aa80fd77 83 while(elevator);
Strikewolf 0:84d5aa80fd77 84 dur = timer.read_us();
Strikewolf 0:84d5aa80fd77 85 return dur;
Strikewolf 0:84d5aa80fd77 86 }
Strikewolf 0:84d5aa80fd77 87
Strikewolf 0:84d5aa80fd77 88 //Primary function for setting motors for radio driving
Strikewolf 0:84d5aa80fd77 89 void radioDrive() {
Strikewolf 0:84d5aa80fd77 90 int radioY = getRadioY();
Strikewolf 0:84d5aa80fd77 91 int radioX = getRadioX();
Strikewolf 0:84d5aa80fd77 92
Strikewolf 0:84d5aa80fd77 93 //Identify drive mode from radio values
Strikewolf 0:84d5aa80fd77 94 if (radioY < CHAN2_MID - DEADZONE)
Strikewolf 0:84d5aa80fd77 95 mode = FORWARD;
Strikewolf 0:84d5aa80fd77 96 if (radioY > CHAN2_MID + DEADZONE)
Strikewolf 0:84d5aa80fd77 97 mode = REVERSE;
Strikewolf 0:84d5aa80fd77 98 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 0:84d5aa80fd77 99 radioX < CHAN1_MID - DEADZONE)
Strikewolf 0:84d5aa80fd77 100 mode = CENTERLEFT;
Strikewolf 0:84d5aa80fd77 101 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 0:84d5aa80fd77 102 radioX > CHAN1_MID + DEADZONE)
Strikewolf 0:84d5aa80fd77 103 mode = CENTERRIGHT;
Strikewolf 0:84d5aa80fd77 104 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 0:84d5aa80fd77 105 radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
Strikewolf 0:84d5aa80fd77 106 mode = STOP;
Strikewolf 0:84d5aa80fd77 107
Strikewolf 0:84d5aa80fd77 108 //Normalize values for PWM magnitude
Strikewolf 0:84d5aa80fd77 109 xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
Strikewolf 0:84d5aa80fd77 110 ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
Strikewolf 0:84d5aa80fd77 111
Strikewolf 0:84d5aa80fd77 112 //Set Motors Accordingly
Strikewolf 0:84d5aa80fd77 113 switch(mode) {
Strikewolf 0:84d5aa80fd77 114 case FORWARD:
Strikewolf 0:84d5aa80fd77 115 //Handle variable turns
Strikewolf 0:84d5aa80fd77 116 if (radioX > CHAN1_MID + DEADZONE) {
Strikewolf 0:84d5aa80fd77 117 rightMotorPWM = ypwm - 0.8*xpwm;
Strikewolf 0:84d5aa80fd77 118 leftMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 119 } else if (radioX < CHAN1_MID - DEADZONE) {
Strikewolf 0:84d5aa80fd77 120 rightMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 121 leftMotorPWM = ypwm - 0.8*xpwm;
Strikewolf 0:84d5aa80fd77 122 } else {
Strikewolf 0:84d5aa80fd77 123 rightMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 124 leftMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 125 }
Strikewolf 0:84d5aa80fd77 126 leftMotor1 = 0;
Strikewolf 0:84d5aa80fd77 127 leftMotor2 = 1;
Strikewolf 0:84d5aa80fd77 128 rightMotor1 = 0;
Strikewolf 0:84d5aa80fd77 129 rightMotor2 = 1;
Strikewolf 0:84d5aa80fd77 130 break;
Strikewolf 0:84d5aa80fd77 131
Strikewolf 0:84d5aa80fd77 132 case REVERSE:
Strikewolf 0:84d5aa80fd77 133 //Handle variable turns
Strikewolf 0:84d5aa80fd77 134 if (radioX > CHAN1_MID + DEADZONE) {
Strikewolf 0:84d5aa80fd77 135 rightMotorPWM = ypwm - 0.8*xpwm;
Strikewolf 0:84d5aa80fd77 136 leftMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 137 } else if (radioX < CHAN1_MID - DEADZONE) {
Strikewolf 0:84d5aa80fd77 138 rightMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 139 leftMotorPWM = ypwm - 0.8*xpwm;
Strikewolf 0:84d5aa80fd77 140 } else {
Strikewolf 0:84d5aa80fd77 141 rightMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 142 leftMotorPWM = ypwm;
Strikewolf 0:84d5aa80fd77 143 }
Strikewolf 0:84d5aa80fd77 144 leftMotor1 = 1;
Strikewolf 0:84d5aa80fd77 145 leftMotor2 = 0;
Strikewolf 0:84d5aa80fd77 146 rightMotor1 = 1;
Strikewolf 0:84d5aa80fd77 147 rightMotor2 = 0;
Strikewolf 0:84d5aa80fd77 148 break;
Strikewolf 0:84d5aa80fd77 149
Strikewolf 0:84d5aa80fd77 150 case CENTERRIGHT:
Strikewolf 0:84d5aa80fd77 151 rightMotorPWM = xpwm;
Strikewolf 0:84d5aa80fd77 152 leftMotorPWM = xpwm;
Strikewolf 0:84d5aa80fd77 153 leftMotor1 = 1;
Strikewolf 0:84d5aa80fd77 154 leftMotor2 = 0;
Strikewolf 0:84d5aa80fd77 155 rightMotor1 = 0;
Strikewolf 0:84d5aa80fd77 156 rightMotor2 = 1;
Strikewolf 0:84d5aa80fd77 157 break;
Strikewolf 0:84d5aa80fd77 158
Strikewolf 0:84d5aa80fd77 159 case CENTERLEFT:
Strikewolf 0:84d5aa80fd77 160 rightMotorPWM = xpwm;
Strikewolf 0:84d5aa80fd77 161 leftMotorPWM = xpwm;
Strikewolf 0:84d5aa80fd77 162 leftMotor1 = 0;
Strikewolf 0:84d5aa80fd77 163 leftMotor2 = 1;
Strikewolf 0:84d5aa80fd77 164 rightMotor1 = 1;
Strikewolf 0:84d5aa80fd77 165 rightMotor2 = 0;
Strikewolf 0:84d5aa80fd77 166 break;
Strikewolf 0:84d5aa80fd77 167
Strikewolf 0:84d5aa80fd77 168 case STOP:
Strikewolf 0:84d5aa80fd77 169 rightMotorPWM = 0;
Strikewolf 0:84d5aa80fd77 170 leftMotorPWM = 0;
Strikewolf 0:84d5aa80fd77 171 leftMotor1 = 0;
Strikewolf 0:84d5aa80fd77 172 leftMotor2 = 1;
Strikewolf 0:84d5aa80fd77 173 rightMotor1 = 1;
Strikewolf 0:84d5aa80fd77 174 rightMotor2 = 0;
Strikewolf 0:84d5aa80fd77 175 break;
Strikewolf 0:84d5aa80fd77 176 }
Strikewolf 0:84d5aa80fd77 177 }
Strikewolf 0:84d5aa80fd77 178
Strikewolf 0:84d5aa80fd77 179 //Transmit position and heading
Strikewolf 0:84d5aa80fd77 180 void xbeeTelemetry() {
Strikewolf 0:84d5aa80fd77 181 xbee.printf("Human Update \r\n");
Strikewolf 0:84d5aa80fd77 182 wait_ms(10);
Strikewolf 0:84d5aa80fd77 183 }
Strikewolf 0:84d5aa80fd77 184
Strikewolf 0:84d5aa80fd77 185 bool isGameOver() {
Strikewolf 0:84d5aa80fd77 186 return xbee.readable();
Strikewolf 0:84d5aa80fd77 187 }
Strikewolf 0:84d5aa80fd77 188
Strikewolf 0:84d5aa80fd77 189
Strikewolf 0:84d5aa80fd77 190
Strikewolf 0:84d5aa80fd77 191 int main() {
Strikewolf 0:84d5aa80fd77 192
Strikewolf 0:84d5aa80fd77 193 //Init Xbee
Strikewolf 0:84d5aa80fd77 194 xbeeReset = 0;
Strikewolf 0:84d5aa80fd77 195 wait_ms(1);
Strikewolf 0:84d5aa80fd77 196 xbeeReset = 1;
Strikewolf 0:84d5aa80fd77 197 wait_ms(1);
Strikewolf 0:84d5aa80fd77 198
Strikewolf 0:84d5aa80fd77 199 while(1) {
Strikewolf 0:84d5aa80fd77 200 xbee.printf("HELLO!\r\n");
Strikewolf 0:84d5aa80fd77 201 wait(0.1);
Strikewolf 0:84d5aa80fd77 202 }
Strikewolf 0:84d5aa80fd77 203 }