Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@9:01c17b286a99, 2014-10-24 (annotated)

Committer:
Hypna
Date:
Fri Oct 24 22:46:24 2014 +0000
Revision:
9:01c17b286a99
Parent:
8:9030d2e3a1e8
Child:
10:210c8f1e3a92
Added log file for debugging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hypna 0:e60f22c1d573 1 #include "DriveController.h"
Hypna 0:e60f22c1d573 2
Hypna 9:01c17b286a99 3 #define MOTOR_SCALE 0.5
Hypna 9:01c17b286a99 4 #define CORRECTION_SCALE 0.09
Hypna 9:01c17b286a99 5
Hypna 9:01c17b286a99 6 DriveController::DriveController() : debugLog("log.txt"), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
Hypna 5:f53f06a866e9 7 wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23),
Hypna 5:f53f06a866e9 8 sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13,
Hypna 5:f53f06a866e9 9 PTC16, PTC17, PTD1, PTD0, PTD5, PTD13)
Hypna 2:d0ce8e26cbc4 10 {
Hypna 4:ac6b2e5b240b 11
Hypna 2:d0ce8e26cbc4 12 }
Hypna 2:d0ce8e26cbc4 13
Hypna 4:ac6b2e5b240b 14 void DriveController::go()
Hypna 0:e60f22c1d573 15 {
Hypna 9:01c17b286a99 16 debugLog << "Starting Drive Controller!" << endl;
Hypna 6:83c1f8d6a65a 17 while(true) //test loop
Hypna 6:83c1f8d6a65a 18 {
Hypna 6:83c1f8d6a65a 19 move();
Hypna 6:83c1f8d6a65a 20 rotate('R');
Hypna 6:83c1f8d6a65a 21 rotate('R');
Hypna 6:83c1f8d6a65a 22 move();
Hypna 6:83c1f8d6a65a 23 }
Hypna 6:83c1f8d6a65a 24
Hypna 6:83c1f8d6a65a 25 /*while(true)
Hypna 2:d0ce8e26cbc4 26 {
Hypna 2:d0ce8e26cbc4 27 getCommand();
Hypna 3:0d7687b6ef14 28
Hypna 5:f53f06a866e9 29 switch(command)
Hypna 5:f53f06a866e9 30 {
Hypna 5:f53f06a866e9 31 case 0: move(); //move forward
Hypna 5:f53f06a866e9 32 break;
Hypna 5:f53f06a866e9 33 case 1: move('B'); //move backward
Hypna 5:f53f06a866e9 34 break;
Hypna 5:f53f06a866e9 35 case 2: rotate('R'); //rotate right
Hypna 5:f53f06a866e9 36 break;
Hypna 5:f53f06a866e9 37 case 3: rotate('L'); //rotate left
Hypna 5:f53f06a866e9 38 }
Hypna 3:0d7687b6ef14 39
Hypna 3:0d7687b6ef14 40 sendComplete();
Hypna 6:83c1f8d6a65a 41 }*/
Hypna 2:d0ce8e26cbc4 42 }
Hypna 2:d0ce8e26cbc4 43
Hypna 5:f53f06a866e9 44 void DriveController::move(char direction)
Hypna 2:d0ce8e26cbc4 45 {
Hypna 5:f53f06a866e9 46 bool atLine = true; //tracks whether the bot is still at the starting intersection
Hypna 5:f53f06a866e9 47 int error = 0;
Hypna 2:d0ce8e26cbc4 48
Hypna 5:f53f06a866e9 49 if(direction == 'B')
Hypna 9:01c17b286a99 50 {
Hypna 9:01c17b286a99 51 debugLog << "Moving Backward" << endl;
Hypna 5:f53f06a866e9 52 wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
Hypna 9:01c17b286a99 53 }
Hypna 5:f53f06a866e9 54 else
Hypna 9:01c17b286a99 55 {
Hypna 9:01c17b286a99 56 debugLog << "Moving Forward" << endl;
Hypna 5:f53f06a866e9 57 wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1;
Hypna 9:01c17b286a99 58 }
Hypna 2:d0ce8e26cbc4 59
Hypna 5:f53f06a866e9 60 do
Hypna 2:d0ce8e26cbc4 61 {
Hypna 6:83c1f8d6a65a 62 sensors.read();
Hypna 6:83c1f8d6a65a 63 sensors.lineDetect(sensorStates);
Hypna 6:83c1f8d6a65a 64
Hypna 9:01c17b286a99 65 debugLog << "Updating Sensors" << endl;
Hypna 9:01c17b286a99 66 debugLog << " 1 2 3 4 5 6 7 8" << endl;
Hypna 9:01c17b286a99 67 debugLog << "----------------------------------" << endl;
Hypna 9:01c17b286a99 68 for(int i = 0; i < 3; i++)
Hypna 9:01c17b286a99 69 {
Hypna 9:01c17b286a99 70 debugLog << i + 1 << " | ";
Hypna 9:01c17b286a99 71 for(int j = 0; j < 8; j++)
Hypna 9:01c17b286a99 72 {
Hypna 9:01c17b286a99 73 debugLog << int(sensorStates[j][i]) << " ";
Hypna 9:01c17b286a99 74 }
Hypna 9:01c17b286a99 75 debugLog << endl;
Hypna 9:01c17b286a99 76 }
Hypna 9:01c17b286a99 77
Hypna 5:f53f06a866e9 78 if(intersection())
Hypna 5:f53f06a866e9 79 {
Hypna 9:01c17b286a99 80 debugLog << "At Intersection" << endl;
Hypna 9:01c17b286a99 81 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
Hypna 5:f53f06a866e9 82 }
Hypna 5:f53f06a866e9 83 else
Hypna 3:0d7687b6ef14 84 {
Hypna 5:f53f06a866e9 85 error = calculateError();
Hypna 9:01c17b286a99 86 debugLog << "Error Calculated : " << error << endl;
Hypna 5:f53f06a866e9 87
Hypna 5:f53f06a866e9 88 if(error < 0)
Hypna 5:f53f06a866e9 89 {
Hypna 9:01c17b286a99 90 wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE;
Hypna 9:01c17b286a99 91 wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
Hypna 6:83c1f8d6a65a 92 }
Hypna 5:f53f06a866e9 93 else
Hypna 5:f53f06a866e9 94 {
Hypna 9:01c17b286a99 95 wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
Hypna 9:01c17b286a99 96 wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*MOTOR_SCALE;
Hypna 5:f53f06a866e9 97 }
Hypna 5:f53f06a866e9 98
Hypna 9:01c17b286a99 99 atLine = false;
Hypna 5:f53f06a866e9 100 }
Hypna 6:83c1f8d6a65a 101 } while(!intersection() || atLine);
Hypna 5:f53f06a866e9 102
Hypna 5:f53f06a866e9 103 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
Hypna 9:01c17b286a99 104 debugLog << "Move Complete" << endl;
Hypna 9:01c17b286a99 105 debugLog << "=====================================" << endl;
Hypna 5:f53f06a866e9 106 }
Hypna 5:f53f06a866e9 107
Hypna 5:f53f06a866e9 108 int DriveController::calculateError()
Hypna 5:f53f06a866e9 109 {
Hypna 5:f53f06a866e9 110 int error;
Hypna 5:f53f06a866e9 111 int bin = 0;
Hypna 5:f53f06a866e9 112
Hypna 5:f53f06a866e9 113 for(int i = 7; i >= 0; i--)
Hypna 5:f53f06a866e9 114 {
Hypna 6:83c1f8d6a65a 115 bin += sensorStates[i][1]<<(7-i);
Hypna 2:d0ce8e26cbc4 116 }
Hypna 3:0d7687b6ef14 117
Hypna 5:f53f06a866e9 118 switch(bin)
Hypna 5:f53f06a866e9 119 {
Hypna 5:f53f06a866e9 120 case 1: error = 7;
Hypna 5:f53f06a866e9 121 break;
Hypna 5:f53f06a866e9 122 case 3: error = 6;
Hypna 5:f53f06a866e9 123 break;
Hypna 5:f53f06a866e9 124 case 2: error = 5;
Hypna 5:f53f06a866e9 125 break;
Hypna 5:f53f06a866e9 126 case 6: error = 4;
Hypna 5:f53f06a866e9 127 break;
Hypna 5:f53f06a866e9 128 case 4: error = 3;
Hypna 5:f53f06a866e9 129 break;
Hypna 5:f53f06a866e9 130 case 12: error = 2;
Hypna 5:f53f06a866e9 131 break;
Hypna 5:f53f06a866e9 132 case 8: error = 1;
Hypna 5:f53f06a866e9 133 break;
Hypna 5:f53f06a866e9 134 case 24: error = 0;
Hypna 5:f53f06a866e9 135 break;
Hypna 5:f53f06a866e9 136 case 16: error = -1;
Hypna 5:f53f06a866e9 137 break;
Hypna 5:f53f06a866e9 138 case 48: error = -2;
Hypna 5:f53f06a866e9 139 break;
Hypna 5:f53f06a866e9 140 case 32: error = -3;
Hypna 5:f53f06a866e9 141 break;
Hypna 5:f53f06a866e9 142 case 96: error = -4;
Hypna 5:f53f06a866e9 143 break;
Hypna 5:f53f06a866e9 144 case 64: error = -5;
Hypna 5:f53f06a866e9 145 break;
Hypna 5:f53f06a866e9 146 case 192: error = -6;
Hypna 5:f53f06a866e9 147 break;
Hypna 5:f53f06a866e9 148 case 128: error = -7;
Hypna 5:f53f06a866e9 149 }
Hypna 5:f53f06a866e9 150
Hypna 5:f53f06a866e9 151 return error;
Hypna 2:d0ce8e26cbc4 152 }
Hypna 2:d0ce8e26cbc4 153
Hypna 4:ac6b2e5b240b 154 bool DriveController::intersection()
Hypna 2:d0ce8e26cbc4 155 {
Hypna 6:83c1f8d6a65a 156
Hypna 6:83c1f8d6a65a 157 // add speed bump check later
Hypna 6:83c1f8d6a65a 158
Hypna 5:f53f06a866e9 159 return (sensorStates[0][1]&&sensorStates[1][1]&&sensorStates[2][1]&&sensorStates[3][1])
Hypna 5:f53f06a866e9 160 || (sensorStates[4][1]&&sensorStates[5][1]&&sensorStates[6][1]&&sensorStates[7][1]);
Hypna 2:d0ce8e26cbc4 161 }
Hypna 1:fa6eb0c33b2f 162
Hypna 5:f53f06a866e9 163 void DriveController::rotate(char direction)
Hypna 2:d0ce8e26cbc4 164 {
Hypna 6:83c1f8d6a65a 165 bool atLine = true;
Hypna 2:d0ce8e26cbc4 166
Hypna 6:83c1f8d6a65a 167 if(direction == 'L')
Hypna 6:83c1f8d6a65a 168 {
Hypna 6:83c1f8d6a65a 169 wheelDirection1 = wheelDirection3 = 0;
Hypna 6:83c1f8d6a65a 170 wheelDirection2 = wheelDirection4 = 1;
Hypna 9:01c17b286a99 171 debugLog << "Rotating Left" << endl;
Hypna 6:83c1f8d6a65a 172 }
Hypna 6:83c1f8d6a65a 173 else
Hypna 6:83c1f8d6a65a 174 {
Hypna 6:83c1f8d6a65a 175 wheelDirection1 = wheelDirection3 = 1;
Hypna 6:83c1f8d6a65a 176 wheelDirection2 = wheelDirection3 = 0;
Hypna 9:01c17b286a99 177 debugLog << "Rotating Right" << endl;
Hypna 6:83c1f8d6a65a 178 }
Hypna 6:83c1f8d6a65a 179
Hypna 9:01c17b286a99 180 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
Hypna 6:83c1f8d6a65a 181
Hypna 6:83c1f8d6a65a 182 do
Hypna 6:83c1f8d6a65a 183 {
Hypna 6:83c1f8d6a65a 184 sensors.read();
Hypna 6:83c1f8d6a65a 185 sensors.lineDetect(sensorStates);
Hypna 6:83c1f8d6a65a 186
Hypna 9:01c17b286a99 187 debugLog << "Updating Sensors" << endl;
Hypna 9:01c17b286a99 188 debugLog << " 1 2 3 4 5 6 7 8" << endl;
Hypna 9:01c17b286a99 189 debugLog << "----------------------------------" << endl;
Hypna 9:01c17b286a99 190 for(int i = 0; i < 3; i++)
Hypna 9:01c17b286a99 191 {
Hypna 9:01c17b286a99 192 debugLog << i + 1 << " | ";
Hypna 9:01c17b286a99 193 for(int j = 0; j < 8; j++)
Hypna 9:01c17b286a99 194 {
Hypna 9:01c17b286a99 195 debugLog << int(sensorStates[j][i]) << " ";
Hypna 9:01c17b286a99 196 }
Hypna 9:01c17b286a99 197 debugLog << endl;
Hypna 9:01c17b286a99 198 }
Hypna 9:01c17b286a99 199
Hypna 6:83c1f8d6a65a 200 if(!intersection())
Hypna 6:83c1f8d6a65a 201 atLine = false;
Hypna 6:83c1f8d6a65a 202
Hypna 6:83c1f8d6a65a 203 } while(!intersection() || atLine);
Hypna 6:83c1f8d6a65a 204
Hypna 6:83c1f8d6a65a 205 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
Hypna 9:01c17b286a99 206
Hypna 9:01c17b286a99 207 debugLog << "Rotate Complete" << endl;
Hypna 9:01c17b286a99 208 debugLog << "=====================================" << endl;
Hypna 9:01c17b286a99 209
Hypna 2:d0ce8e26cbc4 210 }
Hypna 2:d0ce8e26cbc4 211
Hypna 4:ac6b2e5b240b 212 void DriveController::getCommand()
Hypna 2:d0ce8e26cbc4 213 {
Hypna 7:2f3e841ee0ff 214 bool received = false;
Hypna 7:2f3e841ee0ff 215 int status;
Hypna 7:2f3e841ee0ff 216 int msg;
Hypna 2:d0ce8e26cbc4 217
Hypna 7:2f3e841ee0ff 218 while(!received)
Hypna 7:2f3e841ee0ff 219 {
Hypna 7:2f3e841ee0ff 220 status = i2c.receive();
Hypna 7:2f3e841ee0ff 221
Hypna 7:2f3e841ee0ff 222 if(status == 2) //if status is WriteGeneral
Hypna 7:2f3e841ee0ff 223 {
Hypna 7:2f3e841ee0ff 224 msg = i2c.read();
Hypna 7:2f3e841ee0ff 225 received = true;
Hypna 7:2f3e841ee0ff 226 }
Hypna 7:2f3e841ee0ff 227 }
Hypna 7:2f3e841ee0ff 228
Hypna 8:9030d2e3a1e8 229 command = msg & 7;
Hypna 8:9030d2e3a1e8 230 edge = msg & 24;
Hypna 7:2f3e841ee0ff 231
Hypna 2:d0ce8e26cbc4 232 }
Hypna 2:d0ce8e26cbc4 233
Hypna 4:ac6b2e5b240b 234 void DriveController::sendComplete()
Hypna 4:ac6b2e5b240b 235 {
Hypna 7:2f3e841ee0ff 236 i2c.write(1);
Hypna 4:ac6b2e5b240b 237 }