Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp
- Committer:
- jmar11
- Date:
- 2015-04-18
- Revision:
- 16:b49db5c8e16c
- Parent:
- 14:7a4cf2ed2499
- Child:
- 17:5046b27f5441
File content as of revision 16:b49db5c8e16c:
#include "DriveController.h" #define MOTOR_SCALE 0.5 #define CORRECTION_SCALE 0.15 #define Kp 0.066666666667 #define Ki 0.006666666667 DriveController::DriveController() : i2c(PTC2, PTC1), treadSpeed1(PTB0), treadSpeed2(PTB1), treadDirection1(PTE20), treadDirection2(PTE21), 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), orient(NORTH) { } void DriveController::move(typename ::abs_direction direction) { int diff; bool atLine = true; //tracks whether the bot is still at the starting intersection double error = 0; diff=direction-orient; if(abs(diff)==3){ diff/=-3; if(diff == 2)//back { treadDirection1 = treadDirection2 = 0; } else if(direction == 0)//forward { treadDirection1 = treadDirection2 = 1; } do { sensors.lineDetect(sensorStates); if(intersection()) { treadSpeed1 = treadSpeed2 = MOTOR_SCALE; } else { error = calculateError(); if(error < 0) { treadSpeed1 = (1.0 - fabs(error))*MOTOR_SCALE; treadSpeed2 = MOTOR_SCALE; } else { treadSpeed1 = MOTOR_SCALE; treadSpeed2 = (1.0 - error)*MOTOR_SCALE; } atLine = false; } } while(!intersection() || atLine); treadSpeed1 = treadSpeed2 = 0; } double DriveController::calculateError() { double error; int bin = 0; for(int i = 7; i >= 0; i--) { bin += sensorStates[i][0]<<(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][0]&&sensorStates[1][0]&&sensorStates[2][0]&&sensorStates[3][0]) || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]); } void DriveController::rotate(typename ::abs_direction direction) { int diff; bool atLine = true; diff=direction-orient if(abs(diff)==3){ diff/=-3; } do{ if(diff < 0) { treadDirection1 = 0; treadDirection2 = 1; diff++; } else if(diff > 0) { treadDirection1 = 0; treadDirection2 = 1; diff--; } treadSpeed1 = treadSpeed2 = MOTOR_SCALE; }while(diff!=0); /*do { sensors.lineDetect(sensorStates); if(!intersection()) atLine = false; } while(!intersection() || atLine);*/ wait(0.9); treadSpeed1 = treadSpeed2 = 0; } void DriveController::spinTest() { treadDirection1 = 1; treadDirection2 = 0; treadSpeed1 = treadSpeed2 = MOTOR_SCALE; while(true) wait(1); } typename ::abs_direction getOrient() { return orient; }