Texas State IEEE / Mbed 2 deprecated R5

Code to run the microcontrollers on the R5 competition bot

Dependencies:   LineSensors mbed

DriveController.cpp@3:0d7687b6ef14, 2014-10-03 (annotated)

Committer:
Hypna
Date:
Fri Oct 03 18:54:42 2014 +0000
Revision:
3:0d7687b6ef14
Parent:
2:d0ce8e26cbc4
Child:
4:ac6b2e5b240b
did things

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hypna 0:e60f22c1d573 1 #include "DriveController.h"
Hypna 0:e60f22c1d573 2
Hypna 3:0d7687b6ef14 3 #define REFLECTION_THRESHOLD = 0
Hypna 2:d0ce8e26cbc4 4
Hypna 3:0d7687b6ef14 5 /*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7),
Hypna 3:0d7687b6ef14 6 sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11),
Hypna 3:0d7687b6ef14 7 sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2),
Hypna 3:0d7687b6ef14 8 sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0),
Hypna 3:0d7687b6ef14 9 sensor23(PTD5), sensor24(PTD13), orientation(NORTH)
Hypna 2:d0ce8e26cbc4 10 {
Hypna 2:d0ce8e26cbc4 11 command.direction = NORTH;
Hypna 2:d0ce8e26cbc4 12 command.distance = 0;
Hypna 3:0d7687b6ef14 13 }*/
Hypna 3:0d7687b6ef14 14
Hypna 3:0d7687b6ef14 15 DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH)
Hypna 3:0d7687b6ef14 16 {
Hypna 3:0d7687b6ef14 17 command.direction = NORTH;
Hypna 3:0d7687b6ef14 18 command.distance = 0;
Hypna 3:0d7687b6ef14 19
Hypna 3:0d7687b6ef14 20 for(int i = 0; i < 24; i++)
Hypna 3:0d7687b6ef14 21 {
Hypna 3:0d7687b6ef14 22 sensors.push_back(new DigitalInOut(sensorPinNames[i]));
Hypna 3:0d7687b6ef14 23 }
Hypna 2:d0ce8e26cbc4 24 }
Hypna 2:d0ce8e26cbc4 25
Hypna 0:e60f22c1d573 26 DriveController::void go()
Hypna 0:e60f22c1d573 27 {
Hypna 2:d0ce8e26cbc4 28 while(true)
Hypna 2:d0ce8e26cbc4 29 {
Hypna 2:d0ce8e26cbc4 30 getCommand();
Hypna 3:0d7687b6ef14 31
Hypna 2:d0ce8e26cbc4 32 if(currentCommand.distance != 0)
Hypna 2:d0ce8e26cbc4 33 move();
Hypna 3:0d7687b6ef14 34
Hypna 3:0d7687b6ef14 35 sendComplete();
Hypna 2:d0ce8e26cbc4 36 }
Hypna 2:d0ce8e26cbc4 37 }
Hypna 2:d0ce8e26cbc4 38
Hypna 2:d0ce8e26cbc4 39 DriveController::void move()
Hypna 2:d0ce8e26cbc4 40 {
Hypna 2:d0ce8e26cbc4 41 int travelled = 0;
Hypna 3:0d7687b6ef14 42 bool atIntersection = true;
Hypna 2:d0ce8e26cbc4 43
Hypna 2:d0ce8e26cbc4 44 if(orientation != command.direction)
Hypna 2:d0ce8e26cbc4 45 rotate(command.direction);
Hypna 2:d0ce8e26cbc4 46
Hypna 2:d0ce8e26cbc4 47 while(travelled < command.distance)
Hypna 2:d0ce8e26cbc4 48 {
Hypna 2:d0ce8e26cbc4 49 forward(calculateError());
Hypna 3:0d7687b6ef14 50
Hypna 3:0d7687b6ef14 51 if(intersection() && !atIntersection)
Hypna 3:0d7687b6ef14 52 {
Hypna 2:d0ce8e26cbc4 53 travelled++;
Hypna 3:0d7687b6ef14 54 atIntersection = true;
Hypna 3:0d7687b6ef14 55 }
Hypna 3:0d7687b6ef14 56 else if(!intersection())
Hypna 3:0d7687b6ef14 57 atIntersection = false;
Hypna 2:d0ce8e26cbc4 58 }
Hypna 3:0d7687b6ef14 59
Hypna 3:0d7687b6ef14 60 command.distance = 0;
Hypna 2:d0ce8e26cbc4 61 }
Hypna 2:d0ce8e26cbc4 62
Hypna 2:d0ce8e26cbc4 63 DriveController::double calculateError()
Hypna 2:d0ce8e26cbc4 64 {
Hypna 0:e60f22c1d573 65
Hypna 0:e60f22c1d573 66 }
Hypna 0:e60f22c1d573 67
Hypna 2:d0ce8e26cbc4 68 DriveController::void forward(double correction)
Hypna 1:fa6eb0c33b2f 69 {
Hypna 2:d0ce8e26cbc4 70
Hypna 2:d0ce8e26cbc4 71 }
Hypna 2:d0ce8e26cbc4 72
Hypna 2:d0ce8e26cbc4 73 DriveController::bool intersection()
Hypna 2:d0ce8e26cbc4 74 {
Hypna 3:0d7687b6ef14 75 return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17])
Hypna 3:0d7687b6ef14 76 &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20])
Hypna 3:0d7687b6ef14 77 &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22])
Hypna 3:0d7687b6ef14 78 &&(sensorStates[7]||sensorStates[15]||sensorStates[23]);
Hypna 2:d0ce8e26cbc4 79 }
Hypna 1:fa6eb0c33b2f 80
Hypna 2:d0ce8e26cbc4 81 DriveController::void rotate(Direction diretion)
Hypna 2:d0ce8e26cbc4 82 {
Hypna 2:d0ce8e26cbc4 83
Hypna 2:d0ce8e26cbc4 84 }
Hypna 2:d0ce8e26cbc4 85
Hypna 2:d0ce8e26cbc4 86 DriveController::void getCommand()
Hypna 2:d0ce8e26cbc4 87 {
Hypna 2:d0ce8e26cbc4 88
Hypna 2:d0ce8e26cbc4 89 }
Hypna 2:d0ce8e26cbc4 90
Hypna 2:d0ce8e26cbc4 91 DriveControler::void readSensors()
Hypna 2:d0ce8e26cbc4 92 {
Hypna 2:d0ce8e26cbc4 93
Hypna 2:d0ce8e26cbc4 94 }