Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@12:edcae0f36e9c, 2014-11-14 (annotated)

Committer:
Hypna
Date:
Fri Nov 14 18:51:04 2014 +0000
Revision:
12:edcae0f36e9c
Parent:
11:5d20ce95e137
Child:
13:aa6f64c73271
adjusted error correction algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hypna 0:e60f22c1d573 1 #include "DriveController.h"
Hypna 0:e60f22c1d573 2
Hypna 12:edcae0f36e9c 3 #define MOTOR_SCALE 0.4
Hypna 12:edcae0f36e9c 4 #define CORRECTION_SCALE 0.05
Hypna 9:01c17b286a99 5
Hypna 10:210c8f1e3a92 6 DriveController::DriveController() : 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 10:210c8f1e3a92 16 sensors.setThreshold();
Hypna 10:210c8f1e3a92 17
Hypna 10:210c8f1e3a92 18 wait(10);
Hypna 10:210c8f1e3a92 19
Hypna 10:210c8f1e3a92 20 wheelSpeed1.period_us(50); //setting pwm period for all wheels to 20khz
Hypna 10:210c8f1e3a92 21 wheelSpeed2.period_us(50);
Hypna 10:210c8f1e3a92 22 wheelSpeed3.period_us(50);
Hypna 10:210c8f1e3a92 23 wheelSpeed4.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 10:210c8f1e3a92 30 wait(1);
Hypna 6:83c1f8d6a65a 31 rotate('R');
Hypna 10:210c8f1e3a92 32 wait(1);
Hypna 6:83c1f8d6a65a 33 rotate('R');
Hypna 10:210c8f1e3a92 34 wait(1);
Hypna 6:83c1f8d6a65a 35 move();
Hypna 10:210c8f1e3a92 36 wait(1);
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 5:f53f06a866e9 45 case 0: move(); //move forward
Hypna 5:f53f06a866e9 46 break;
Hypna 5:f53f06a866e9 47 case 1: move('B'); //move backward
Hypna 5:f53f06a866e9 48 break;
Hypna 5:f53f06a866e9 49 case 2: rotate('R'); //rotate right
Hypna 5:f53f06a866e9 50 break;
Hypna 5:f53f06a866e9 51 case 3: 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 11:5d20ce95e137 65 wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
Hypna 9:01c17b286a99 66 }
Hypna 5:f53f06a866e9 67 else
Hypna 9:01c17b286a99 68 {
Hypna 11:5d20ce95e137 69 wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 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 9:01c17b286a99 78 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 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 12:edcae0f36e9c 86 wheelSpeed1 = wheelSpeed3 = (1.0 - fabs(error))*MOTOR_SCALE;
Hypna 12:edcae0f36e9c 87 wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
Hypna 6:83c1f8d6a65a 88 }
Hypna 5:f53f06a866e9 89 else
Hypna 5:f53f06a866e9 90 {
Hypna 12:edcae0f36e9c 91 wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
Hypna 12:edcae0f36e9c 92 wheelSpeed2 = wheelSpeed4 = (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 5:f53f06a866e9 99 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 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 6:83c1f8d6a65a 109 bin += sensorStates[i][1]<<(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 5:f53f06a866e9 155 return (sensorStates[0][1]&&sensorStates[1][1]&&sensorStates[2][1]&&sensorStates[3][1])
Hypna 5:f53f06a866e9 156 || (sensorStates[4][1]&&sensorStates[5][1]&&sensorStates[6][1]&&sensorStates[7][1]);
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 6:83c1f8d6a65a 165 wheelDirection1 = wheelDirection3 = 0;
Hypna 6:83c1f8d6a65a 166 wheelDirection2 = wheelDirection4 = 1;
Hypna 6:83c1f8d6a65a 167 }
Hypna 6:83c1f8d6a65a 168 else
Hypna 6:83c1f8d6a65a 169 {
Hypna 10:210c8f1e3a92 170 wheelDirection1 = wheelDirection3 = 0;
Hypna 10:210c8f1e3a92 171 wheelDirection2 = wheelDirection4 = 1;
Hypna 6:83c1f8d6a65a 172 }
Hypna 6:83c1f8d6a65a 173
Hypna 9:01c17b286a99 174 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 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 12:edcae0f36e9c 183 } while(!intersection() || atLine);*/
Hypna 12:edcae0f36e9c 184
Hypna 12:edcae0f36e9c 185 wait(0.6);
Hypna 6:83c1f8d6a65a 186
Hypna 6:83c1f8d6a65a 187 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
Hypna 2:d0ce8e26cbc4 188 }
Hypna 2:d0ce8e26cbc4 189
Hypna 4:ac6b2e5b240b 190 void DriveController::getCommand()
Hypna 2:d0ce8e26cbc4 191 {
Hypna 7:2f3e841ee0ff 192 bool received = false;
Hypna 7:2f3e841ee0ff 193 int status;
Hypna 7:2f3e841ee0ff 194 int msg;
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 7:2f3e841ee0ff 202 msg = i2c.read();
Hypna 7:2f3e841ee0ff 203 received = true;
Hypna 7:2f3e841ee0ff 204 }
Hypna 7:2f3e841ee0ff 205 }
Hypna 7:2f3e841ee0ff 206
Hypna 8:9030d2e3a1e8 207 command = msg & 7;
Hypna 8:9030d2e3a1e8 208 edge = msg & 24;
Hypna 7:2f3e841ee0ff 209
Hypna 2:d0ce8e26cbc4 210 }
Hypna 2:d0ce8e26cbc4 211
Hypna 4:ac6b2e5b240b 212 void DriveController::sendComplete()
Hypna 4:ac6b2e5b240b 213 {
Hypna 7:2f3e841ee0ff 214 i2c.write(1);
Hypna 4:ac6b2e5b240b 215 }
Hypna 12:edcae0f36e9c 216
Hypna 12:edcae0f36e9c 217 void DriveController::spinTest()
Hypna 12:edcae0f36e9c 218 {
Hypna 12:edcae0f36e9c 219 wheelDirection1 = wheelDirection3 = 1;
Hypna 12:edcae0f36e9c 220 wheelDirection2 = wheelDirection4 = 0;
Hypna 12:edcae0f36e9c 221
Hypna 12:edcae0f36e9c 222 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
Hypna 12:edcae0f36e9c 223
Hypna 12:edcae0f36e9c 224 while(true)
Hypna 12:edcae0f36e9c 225 wait(1);
Hypna 12:edcae0f36e9c 226 }