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?
User | Revision | Line number | New 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 | } |