![](/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-14
- Revision:
- 4:ac6b2e5b240b
- Parent:
- 3:0d7687b6ef14
- Child:
- 5:f53f06a866e9
File content as of revision 4:ac6b2e5b240b:
#include "DriveController.h" #define REFLECTION_THRESHOLD = 0 DriveController::DriveController() : orientation(NORTH), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2), wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23) { PinName sensorPins[] = {PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13, PTC16, PTC17, PTD2, PTD0, PTD5, PTD13}; command.direction = NORTH; command.distance = 0; for(int i = 0; i < 24; i++) { sensors.push_back(DigitalInOut(sensorPins[i])); } } void DriveController::go() { while(true) { getCommand(); if(command.distance != 0) move(); sendComplete(); } } void DriveController::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; } double DriveController::calculateError() { return 0; } void DriveController::forward(double correction) { } bool DriveController::intersection() { return sensorStates[8]&&sensorStates[9]&&sensorStates[10]&&sensorStates[12]&&sensorStates[13]&&sensorStates[14]&&sensorStates[15]; } void DriveController::rotate(Direction diretion) { } void DriveController::getCommand() { } void DriveController::sendComplete() { } void DriveController::readSensors() { }