Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@14:7a4cf2ed2499, 2015-04-15 (annotated)

Committer:
Hypna
Date:
Wed Apr 15 14:40:08 2015 +0000
Revision:
14:7a4cf2ed2499
Parent:
13:aa6f64c73271
Child:
15:150ec9448efc
Child:
16:b49db5c8e16c
adjusting test loop

Who changed what in which revision?

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