Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@17:5046b27f5441, 2015-04-18 (annotated)

Committer:
jmar11
Date:
Sat Apr 18 03:02:28 2015 +0000
Revision:
17:5046b27f5441
Parent:
16:b49db5c8e16c
Child:
18:2bd595af51d2
Jaime's library

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