Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@22:8f726dc175cd, 2015-04-18 (annotated)

Committer:
Hypna
Date:
Sat Apr 18 04:57:20 2015 +0000
Revision:
22:8f726dc175cd
Parent:
18:2bd595af51d2 
stuff

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,
jmar11 16:b49db5c8e16c 11 PTC16, PTC17, PTD1, PTD0, PTD5, PTD13), orient(NORTH)
Hypna 2:d0ce8e26cbc4 12 {
jmar11 18:2bd595af51d2 13 sensors.setThreshold();
jmar11 18:2bd595af51d2 14 wait(10);
jmar11 18:2bd595af51d2 15 treadSpeed1.period_us(50); //setting pwm period for all wheels to 20khz
jmar11 18:2bd595af51d2 16 treadSpeed2.period_us(50);
Hypna 2:d0ce8e26cbc4 17 }
Hypna 2:d0ce8e26cbc4 18
jmar11 17:5046b27f5441 19 void DriveController::move(typename ::abs_direction dir)
Hypna 0:e60f22c1d573 20 {
jmar11 16:b49db5c8e16c 21 int diff;
Hypna 5:f53f06a866e9 22 bool atLine = true; //tracks whether the bot is still at the starting intersection
Hypna 12:edcae0f36e9c 23 double error = 0;
Hypna 2:d0ce8e26cbc4 24
jmar11 16:b49db5c8e16c 25 diff=direction-orient;
jmar11 16:b49db5c8e16c 26
jmar11 16:b49db5c8e16c 27 if(abs(diff)==3){
jmar11 16:b49db5c8e16c 28 diff/=-3;
jmar11 16:b49db5c8e16c 29
jmar11 16:b49db5c8e16c 30 if(diff == 2)//back
Hypna 10:210c8f1e3a92 31 {
Hypna 13:aa6f64c73271 32 treadDirection1 = treadDirection2 = 0;
Hypna 9:01c17b286a99 33 }
jmar11 16:b49db5c8e16c 34 else if(direction == 0)//forward
Hypna 9:01c17b286a99 35 {
Hypna 13:aa6f64c73271 36 treadDirection1 = treadDirection2 = 1;
Hypna 9:01c17b286a99 37 }
Hypna 2:d0ce8e26cbc4 38
Hypna 5:f53f06a866e9 39 do
Hypna 2:d0ce8e26cbc4 40 {
Hypna 6:83c1f8d6a65a 41 sensors.lineDetect(sensorStates);
Hypna 6:83c1f8d6a65a 42
Hypna 5:f53f06a866e9 43 if(intersection())
Hypna 5:f53f06a866e9 44 {
Hypna 13:aa6f64c73271 45 treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
Hypna 5:f53f06a866e9 46 }
Hypna 5:f53f06a866e9 47 else
Hypna 3:0d7687b6ef14 48 {
Hypna 5:f53f06a866e9 49 error = calculateError();
Hypna 5:f53f06a866e9 50
Hypna 5:f53f06a866e9 51 if(error < 0)
Hypna 5:f53f06a866e9 52 {
Hypna 13:aa6f64c73271 53 treadSpeed1 = (1.0 - fabs(error))*MOTOR_SCALE;
Hypna 13:aa6f64c73271 54 treadSpeed2 = MOTOR_SCALE;
Hypna 6:83c1f8d6a65a 55 }
Hypna 5:f53f06a866e9 56 else
Hypna 5:f53f06a866e9 57 {
Hypna 13:aa6f64c73271 58 treadSpeed1 = MOTOR_SCALE;
Hypna 13:aa6f64c73271 59 treadSpeed2 = (1.0 - error)*MOTOR_SCALE;
Hypna 5:f53f06a866e9 60 }
Hypna 5:f53f06a866e9 61
Hypna 9:01c17b286a99 62 atLine = false;
Hypna 5:f53f06a866e9 63 }
Hypna 6:83c1f8d6a65a 64 } while(!intersection() || atLine);
Hypna 5:f53f06a866e9 65
Hypna 13:aa6f64c73271 66 treadSpeed1 = treadSpeed2 = 0;
Hypna 5:f53f06a866e9 67 }
Hypna 5:f53f06a866e9 68
Hypna 12:edcae0f36e9c 69 double DriveController::calculateError()
Hypna 5:f53f06a866e9 70 {
Hypna 12:edcae0f36e9c 71 double error;
Hypna 5:f53f06a866e9 72 int bin = 0;
Hypna 5:f53f06a866e9 73
Hypna 5:f53f06a866e9 74 for(int i = 7; i >= 0; i--)
Hypna 5:f53f06a866e9 75 {
Hypna 13:aa6f64c73271 76 bin += sensorStates[i][0]<<(7-i);
Hypna 2:d0ce8e26cbc4 77 }
Hypna 3:0d7687b6ef14 78
Hypna 5:f53f06a866e9 79 switch(bin)
Hypna 5:f53f06a866e9 80 {
Hypna 12:edcae0f36e9c 81 case 1: error = 6;
Hypna 5:f53f06a866e9 82 break;
Hypna 12:edcae0f36e9c 83 case 3: error = 5;
Hypna 5:f53f06a866e9 84 break;
Hypna 12:edcae0f36e9c 85 case 2: error = 4;
Hypna 5:f53f06a866e9 86 break;
Hypna 12:edcae0f36e9c 87 case 6: error = 3;
Hypna 5:f53f06a866e9 88 break;
Hypna 12:edcae0f36e9c 89 case 4: error = 2;
Hypna 5:f53f06a866e9 90 break;
Hypna 12:edcae0f36e9c 91 case 12: error = 1;
Hypna 5:f53f06a866e9 92 break;
Hypna 12:edcae0f36e9c 93 case 8: error = 0;
Hypna 5:f53f06a866e9 94 break;
Hypna 5:f53f06a866e9 95 case 24: error = 0;
Hypna 5:f53f06a866e9 96 break;
Hypna 12:edcae0f36e9c 97 case 16: error = 0;
Hypna 5:f53f06a866e9 98 break;
Hypna 12:edcae0f36e9c 99 case 48: error = -1;
Hypna 5:f53f06a866e9 100 break;
Hypna 12:edcae0f36e9c 101 case 32: error = -2;
Hypna 5:f53f06a866e9 102 break;
Hypna 12:edcae0f36e9c 103 case 96: error = -3;
Hypna 5:f53f06a866e9 104 break;
Hypna 12:edcae0f36e9c 105 case 64: error = -4;
Hypna 5:f53f06a866e9 106 break;
Hypna 12:edcae0f36e9c 107 case 192: error = -5;
Hypna 5:f53f06a866e9 108 break;
Hypna 12:edcae0f36e9c 109 case 128: error = -6;
Hypna 10:210c8f1e3a92 110 break;
Hypna 10:210c8f1e3a92 111 default: error = 0;
Hypna 5:f53f06a866e9 112 }
Hypna 5:f53f06a866e9 113
Hypna 12:edcae0f36e9c 114 return error*CORRECTION_SCALE;
Hypna 2:d0ce8e26cbc4 115 }
Hypna 2:d0ce8e26cbc4 116
Hypna 4:ac6b2e5b240b 117 bool DriveController::intersection()
Hypna 2:d0ce8e26cbc4 118 {
Hypna 6:83c1f8d6a65a 119
Hypna 6:83c1f8d6a65a 120 // add speed bump check later
Hypna 6:83c1f8d6a65a 121
Hypna 13:aa6f64c73271 122 return (sensorStates[0][0]&&sensorStates[1][0]&&sensorStates[2][0]&&sensorStates[3][0])
Hypna 13:aa6f64c73271 123 || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]);
Hypna 2:d0ce8e26cbc4 124 }
Hypna 1:fa6eb0c33b2f 125
jmar11 17:5046b27f5441 126 void DriveController::rotate(typename ::abs_direction dir)
Hypna 2:d0ce8e26cbc4 127 {
jmar11 16:b49db5c8e16c 128 int diff;
Hypna 6:83c1f8d6a65a 129 bool atLine = true;
Hypna 2:d0ce8e26cbc4 130
jmar11 16:b49db5c8e16c 131 diff=direction-orient
jmar11 16:b49db5c8e16c 132
jmar11 16:b49db5c8e16c 133 if(abs(diff)==3){
jmar11 16:b49db5c8e16c 134 diff/=-3;
Hypna 6:83c1f8d6a65a 135 }
jmar11 16:b49db5c8e16c 136 do{
jmar11 16:b49db5c8e16c 137 if(diff < 0)
jmar11 16:b49db5c8e16c 138 {
jmar11 16:b49db5c8e16c 139 treadDirection1 = 0;
jmar11 16:b49db5c8e16c 140 treadDirection2 = 1;
jmar11 16:b49db5c8e16c 141 diff++;
jmar11 16:b49db5c8e16c 142 }
jmar11 16:b49db5c8e16c 143 else if(diff > 0)
jmar11 16:b49db5c8e16c 144 {
jmar11 16:b49db5c8e16c 145 treadDirection1 = 0;
jmar11 16:b49db5c8e16c 146 treadDirection2 = 1;
jmar11 16:b49db5c8e16c 147 diff--;
jmar11 16:b49db5c8e16c 148 }
jmar11 16:b49db5c8e16c 149
jmar11 16:b49db5c8e16c 150 treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
jmar11 16:b49db5c8e16c 151 }while(diff!=0);
Hypna 6:83c1f8d6a65a 152
Hypna 12:edcae0f36e9c 153 /*do
Hypna 6:83c1f8d6a65a 154 {
Hypna 6:83c1f8d6a65a 155 sensors.lineDetect(sensorStates);
Hypna 6:83c1f8d6a65a 156
Hypna 6:83c1f8d6a65a 157 if(!intersection())
Hypna 6:83c1f8d6a65a 158 atLine = false;
Hypna 6:83c1f8d6a65a 159
Hypna 14:7a4cf2ed2499 160 }
Hypna 14:7a4cf2ed2499 161 while(!intersection() || atLine);*/
Hypna 12:edcae0f36e9c 162
Hypna 14:7a4cf2ed2499 163 wait(0.9);
Hypna 6:83c1f8d6a65a 164
Hypna 13:aa6f64c73271 165 treadSpeed1 = treadSpeed2 = 0;
Hypna 2:d0ce8e26cbc4 166 }
Hypna 2:d0ce8e26cbc4 167
Hypna 12:edcae0f36e9c 168 void DriveController::spinTest()
Hypna 12:edcae0f36e9c 169 {
Hypna 13:aa6f64c73271 170 treadDirection1 = 1;
Hypna 13:aa6f64c73271 171 treadDirection2 = 0;
Hypna 12:edcae0f36e9c 172
Hypna 13:aa6f64c73271 173 treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
Hypna 12:edcae0f36e9c 174
Hypna 12:edcae0f36e9c 175 while(true)
Hypna 12:edcae0f36e9c 176 wait(1);
Hypna 12:edcae0f36e9c 177 }
jmar11 16:b49db5c8e16c 178
jmar11 16:b49db5c8e16c 179 typename ::abs_direction getOrient()
jmar11 16:b49db5c8e16c 180 {
jmar11 16:b49db5c8e16c 181 return orient;
jmar11 16:b49db5c8e16c 182 }