Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp
- Committer:
- Hypna
- Date:
- 2014-10-16
- Revision:
- 5:f53f06a866e9
- Parent:
- 4:ac6b2e5b240b
- Child:
- 6:83c1f8d6a65a
File content as of revision 5:f53f06a866e9:
#include "DriveController.h" #include <math> DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2), wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23), sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13, PTC16, PTC17, PTD1, PTD0, PTD5, PTD13) { } void DriveController::go() { while(true) { getCommand(); switch(command) { case 0: move(); //move forward break; case 1: move('B'); //move backward break; case 2: rotate('R'); //rotate right break; case 3: rotate('L'); //rotate left } sendComplete(); } } void DriveController::move(char direction) { bool atLine = true; //tracks whether the bot is still at the starting intersection int error = 0; if(direction == 'B') wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0; else wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1; do { if(intersection()) { wheelSpeed1 = wheelSpeed3 = 1; wheelSpeed2 = wheelSpeed4 = 1; } else { sensors.read(); sensors.lineDetect(sensorStates); error = calculateError(); if(error < 0) { wheelSpeed1 = wheelSpeed3 = 1.0 - abs(error)*0.09; wheelSpeed2 = wheelSpeed4 = 1; { else { wheelSpeed1 = wheelSpeed3 = 1; wheelSpeed2 = wheelSpeed4 = 1.0 - error*0.09; } if(atLine) atLine = false; } } while(!intersection() || atLine) wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0; } int DriveController::calculateError() { int error; int bin = 0; for(int i = 7; i >= 0; i--) { if(sensorStates[i][1] == 1) bin += pow(2, 7-i); } switch(bin) { case 1: error = 7; break; case 3: error = 6; break; case 2: error = 5; break; case 6: error = 4; break; case 4: error = 3; break; case 12: error = 2; break; case 8: error = 1; break; case 24: error = 0; break; case 16: error = -1; break; case 48: error = -2; break; case 32: error = -3; break; case 96: error = -4; break; case 64: error = -5; break; case 192: error = -6; break; case 128: error = -7; } return error; } bool DriveController::intersection() { return (sensorStates[0][1]&&sensorStates[1][1]&&sensorStates[2][1]&&sensorStates[3][1]) || (sensorStates[4][1]&&sensorStates[5][1]&&sensorStates[6][1]&&sensorStates[7][1]); } void DriveController::rotate(char direction) { } void DriveController::getCommand() { } void DriveController::sendComplete() { }