![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp
- Committer:
- Hypna
- Date:
- 2014-10-03
- Revision:
- 3:0d7687b6ef14
- Parent:
- 2:d0ce8e26cbc4
- Child:
- 4:ac6b2e5b240b
File content as of revision 3:0d7687b6ef14:
#include "DriveController.h" #define REFLECTION_THRESHOLD = 0 /*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7), sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11), sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2), sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0), sensor23(PTD5), sensor24(PTD13), orientation(NORTH) { command.direction = NORTH; command.distance = 0; }*/ DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH) { command.direction = NORTH; command.distance = 0; for(int i = 0; i < 24; i++) { sensors.push_back(new DigitalInOut(sensorPinNames[i])); } } DriveController::void go() { while(true) { getCommand(); if(currentCommand.distance != 0) move(); sendComplete(); } } DriveController::void move() { int travelled = 0; bool atIntersection = true; if(orientation != command.direction) rotate(command.direction); while(travelled < command.distance) { forward(calculateError()); if(intersection() && !atIntersection) { travelled++; atIntersection = true; } else if(!intersection()) atIntersection = false; } command.distance = 0; } DriveController::double calculateError() { } DriveController::void forward(double correction) { } DriveController::bool intersection() { return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17]) &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20]) &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22]) &&(sensorStates[7]||sensorStates[15]||sensorStates[23]); } DriveController::void rotate(Direction diretion) { } DriveController::void getCommand() { } DriveControler::void readSensors() { }