![](/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-17
- Revision:
- 7:2f3e841ee0ff
- Parent:
- 6:83c1f8d6a65a
- Child:
- 8:9030d2e3a1e8
File content as of revision 7:2f3e841ee0ff:
#include "DriveController.h" 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) //test loop { move(); rotate('R'); rotate('R'); move(); } /*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 { sensors.read(); sensors.lineDetect(sensorStates); if(intersection()) { wheelSpeed1 = wheelSpeed3 = 1; wheelSpeed2 = wheelSpeed4 = 1; } else { 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--) { bin += sensorStates[i][1]<<(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() { // add speed bump check later 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) { bool atLine = true; if(direction == 'L') { wheelDirection1 = wheelDirection3 = 0; wheelDirection2 = wheelDirection4 = 1; } else { wheelDirection1 = wheelDirection3 = 1; wheelDirection2 = wheelDirection3 = 0; } wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 1; do { sensors.read(); sensors.lineDetect(sensorStates); if(!intersection()) atLine = false; } while(!intersection() || atLine); wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0; } void DriveController::getCommand() { bool received = false; int status; int msg; while(!received) { status = i2c.receive(); if(status == 2) //if status is WriteGeneral { msg = i2c.read(); received = true; } } command = msg & 252; edge = msg & 243; } void DriveController::sendComplete() { i2c.write(1); }