Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@7:2f3e841ee0ff, 2014-10-17 (annotated)

Committer:
Hypna
Date:
Fri Oct 17 18:55:55 2014 +0000
Revision:
7:2f3e841ee0ff
Parent:
6:83c1f8d6a65a
Child:
8:9030d2e3a1e8
added i2c

Who changed what in which revision?

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