Texas State IEEE / Mbed 2 deprecated R5

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   LineSensors mbed

DriveController.cpp@10:210c8f1e3a92, 2014-11-03 (annotated)

Committer:
Hypna
Date:
Mon Nov 03 16:51:27 2014 +0000
Revision:
10:210c8f1e3a92
Parent:
9:01c17b286a99
Child:
11:5d20ce95e137
Removed Debug Log until we get a flash drive setup.; Changed pwm frequency for wheels to 20khz.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hypna 0:e60f22c1d573 1 #include "DriveController.h"
Hypna 0:e60f22c1d573 2
Hypna 10:210c8f1e3a92 3 #define MOTOR_SCALE 0.25
Hypna 10:210c8f1e3a92 4 #define CORRECTION_SCALE 0.01
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 6:83c1f8d6a65a 25 while(true) //test loop
Hypna 6:83c1f8d6a65a 26 {
Hypna 6:83c1f8d6a65a 27 move();
Hypna 10:210c8f1e3a92 28 wait(1);
Hypna 6:83c1f8d6a65a 29 rotate('R');
Hypna 10:210c8f1e3a92 30 wait(1);
Hypna 6:83c1f8d6a65a 31 rotate('R');
Hypna 10:210c8f1e3a92 32 wait(1);
Hypna 6:83c1f8d6a65a 33 move();
Hypna 10:210c8f1e3a92 34 wait(1);
Hypna 6:83c1f8d6a65a 35 }
Hypna 6:83c1f8d6a65a 36
Hypna 6:83c1f8d6a65a 37 /*while(true)
Hypna 2:d0ce8e26cbc4 38 {
Hypna 2:d0ce8e26cbc4 39 getCommand();
Hypna 3:0d7687b6ef14 40
Hypna 5:f53f06a866e9 41 switch(command)
Hypna 5:f53f06a866e9 42 {
Hypna 5:f53f06a866e9 43 case 0: move(); //move forward
Hypna 5:f53f06a866e9 44 break;
Hypna 5:f53f06a866e9 45 case 1: move('B'); //move backward
Hypna 5:f53f06a866e9 46 break;
Hypna 5:f53f06a866e9 47 case 2: rotate('R'); //rotate right
Hypna 5:f53f06a866e9 48 break;
Hypna 5:f53f06a866e9 49 case 3: rotate('L'); //rotate left
Hypna 5:f53f06a866e9 50 }
Hypna 3:0d7687b6ef14 51
Hypna 3:0d7687b6ef14 52 sendComplete();
Hypna 6:83c1f8d6a65a 53 }*/
Hypna 2:d0ce8e26cbc4 54 }
Hypna 2:d0ce8e26cbc4 55
Hypna 5:f53f06a866e9 56 void DriveController::move(char direction)
Hypna 2:d0ce8e26cbc4 57 {
Hypna 5:f53f06a866e9 58 bool atLine = true; //tracks whether the bot is still at the starting intersection
Hypna 5:f53f06a866e9 59 int error = 0;
Hypna 2:d0ce8e26cbc4 60
Hypna 5:f53f06a866e9 61 if(direction == 'B')
Hypna 10:210c8f1e3a92 62 {
Hypna 10:210c8f1e3a92 63 wheelDirection1 = wheelDirection3 = 1;
Hypna 10:210c8f1e3a92 64 wheelDirection2 = wheelDirection4 = 0;
Hypna 9:01c17b286a99 65 }
Hypna 5:f53f06a866e9 66 else
Hypna 9:01c17b286a99 67 {
Hypna 10:210c8f1e3a92 68 wheelDirection1 = wheelDirection3 = 0;
Hypna 10:210c8f1e3a92 69 wheelDirection2 = 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 10:210c8f1e3a92 86 //wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE;
Hypna 10:210c8f1e3a92 87 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
Hypna 6:83c1f8d6a65a 88 }
Hypna 5:f53f06a866e9 89 else
Hypna 5:f53f06a866e9 90 {
Hypna 10:210c8f1e3a92 91 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
Hypna 10:210c8f1e3a92 92 //wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*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 5:f53f06a866e9 102 int DriveController::calculateError()
Hypna 5:f53f06a866e9 103 {
Hypna 5:f53f06a866e9 104 int 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 5:f53f06a866e9 114 case 1: error = 7;
Hypna 5:f53f06a866e9 115 break;
Hypna 5:f53f06a866e9 116 case 3: error = 6;
Hypna 5:f53f06a866e9 117 break;
Hypna 5:f53f06a866e9 118 case 2: error = 5;
Hypna 5:f53f06a866e9 119 break;
Hypna 5:f53f06a866e9 120 case 6: error = 4;
Hypna 5:f53f06a866e9 121 break;
Hypna 5:f53f06a866e9 122 case 4: error = 3;
Hypna 5:f53f06a866e9 123 break;
Hypna 5:f53f06a866e9 124 case 12: error = 2;
Hypna 5:f53f06a866e9 125 break;
Hypna 5:f53f06a866e9 126 case 8: error = 1;
Hypna 5:f53f06a866e9 127 break;
Hypna 5:f53f06a866e9 128 case 24: error = 0;
Hypna 5:f53f06a866e9 129 break;
Hypna 5:f53f06a866e9 130 case 16: error = -1;
Hypna 5:f53f06a866e9 131 break;
Hypna 5:f53f06a866e9 132 case 48: error = -2;
Hypna 5:f53f06a866e9 133 break;
Hypna 5:f53f06a866e9 134 case 32: error = -3;
Hypna 5:f53f06a866e9 135 break;
Hypna 5:f53f06a866e9 136 case 96: error = -4;
Hypna 5:f53f06a866e9 137 break;
Hypna 5:f53f06a866e9 138 case 64: error = -5;
Hypna 5:f53f06a866e9 139 break;
Hypna 5:f53f06a866e9 140 case 192: error = -6;
Hypna 5:f53f06a866e9 141 break;
Hypna 5:f53f06a866e9 142 case 128: error = -7;
Hypna 10:210c8f1e3a92 143 break;
Hypna 10:210c8f1e3a92 144 default: error = 0;
Hypna 5:f53f06a866e9 145 }
Hypna 5:f53f06a866e9 146
Hypna 5:f53f06a866e9 147 return error;
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 6:83c1f8d6a65a 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 6:83c1f8d6a65a 183 } while(!intersection() || atLine);
Hypna 6:83c1f8d6a65a 184
Hypna 6:83c1f8d6a65a 185 wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
Hypna 2:d0ce8e26cbc4 186 }
Hypna 2:d0ce8e26cbc4 187
Hypna 4:ac6b2e5b240b 188 void DriveController::getCommand()
Hypna 2:d0ce8e26cbc4 189 {
Hypna 7:2f3e841ee0ff 190 bool received = false;
Hypna 7:2f3e841ee0ff 191 int status;
Hypna 7:2f3e841ee0ff 192 int msg;
Hypna 2:d0ce8e26cbc4 193
Hypna 7:2f3e841ee0ff 194 while(!received)
Hypna 7:2f3e841ee0ff 195 {
Hypna 7:2f3e841ee0ff 196 status = i2c.receive();
Hypna 7:2f3e841ee0ff 197
Hypna 7:2f3e841ee0ff 198 if(status == 2) //if status is WriteGeneral
Hypna 7:2f3e841ee0ff 199 {
Hypna 7:2f3e841ee0ff 200 msg = i2c.read();
Hypna 7:2f3e841ee0ff 201 received = true;
Hypna 7:2f3e841ee0ff 202 }
Hypna 7:2f3e841ee0ff 203 }
Hypna 7:2f3e841ee0ff 204
Hypna 8:9030d2e3a1e8 205 command = msg & 7;
Hypna 8:9030d2e3a1e8 206 edge = msg & 24;
Hypna 7:2f3e841ee0ff 207
Hypna 2:d0ce8e26cbc4 208 }
Hypna 2:d0ce8e26cbc4 209
Hypna 4:ac6b2e5b240b 210 void DriveController::sendComplete()
Hypna 4:ac6b2e5b240b 211 {
Hypna 7:2f3e841ee0ff 212 i2c.write(1);
Hypna 4:ac6b2e5b240b 213 }