Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp
- Committer:
- Hypna
- Date:
- 2014-11-14
- Revision:
- 12:edcae0f36e9c
- Parent:
- 11:5d20ce95e137
- Child:
- 13:aa6f64c73271
File content as of revision 12:edcae0f36e9c:
#include "DriveController.h" #define MOTOR_SCALE 0.4 #define CORRECTION_SCALE 0.05 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() { sensors.setThreshold(); wait(10); wheelSpeed1.period_us(50); //setting pwm period for all wheels to 20khz wheelSpeed2.period_us(50); wheelSpeed3.period_us(50); wheelSpeed4.period_us(50); //spinTest(); while(true) //test loop { move(); wait(1); rotate('R'); wait(1); rotate('R'); wait(1); move(); wait(1); } /*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 double error = 0; if(direction == 'B') { wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0; } else { wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1; } do { sensors.lineDetect(sensorStates); if(intersection()) { wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE; } else { error = calculateError(); if(error < 0) { wheelSpeed1 = wheelSpeed3 = (1.0 - fabs(error))*MOTOR_SCALE; wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE; } else { wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE; wheelSpeed2 = wheelSpeed4 = (1.0 - error)*MOTOR_SCALE; } atLine = false; } } while(!intersection() || atLine); wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0; } double DriveController::calculateError() { double error; int bin = 0; for(int i = 7; i >= 0; i--) { bin += sensorStates[i][1]<<(7-i); } switch(bin) { case 1: error = 6; break; case 3: error = 5; break; case 2: error = 4; break; case 6: error = 3; break; case 4: error = 2; break; case 12: error = 1; break; case 8: error = 0; break; case 24: error = 0; break; case 16: error = 0; break; case 48: error = -1; break; case 32: error = -2; break; case 96: error = -3; break; case 64: error = -4; break; case 192: error = -5; break; case 128: error = -6; break; default: error = 0; } return error*CORRECTION_SCALE; } 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 = 0; wheelDirection2 = wheelDirection4 = 1; } wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE; /*do { sensors.lineDetect(sensorStates); if(!intersection()) atLine = false; } while(!intersection() || atLine);*/ wait(0.6); 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 & 7; edge = msg & 24; } void DriveController::sendComplete() { i2c.write(1); } void DriveController::spinTest() { wheelDirection1 = wheelDirection3 = 1; wheelDirection2 = wheelDirection4 = 0; wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE; while(true) wait(1); }