![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: DriveController.cpp
- Revision:
- 16:b49db5c8e16c
- Parent:
- 14:7a4cf2ed2499
- Child:
- 17:5046b27f5441
diff -r 7a4cf2ed2499 -r b49db5c8e16c DriveController.cpp --- a/DriveController.cpp Wed Apr 15 14:40:08 2015 +0000 +++ b/DriveController.cpp Sat Apr 18 02:50:24 2015 +0000 @@ -8,63 +8,27 @@ 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) + PTC16, PTC17, PTD1, PTD0, PTD5, PTD13), orient(NORTH) { } -void DriveController::go() +void DriveController::move(typename ::abs_direction direction) { - sensors.setThreshold(); - - wait(10); - - treadSpeed1.period_us(50); //setting pwm period for all wheels to 20khz - treadSpeed2.period_us(50); - - //spinTest(); - - while(true) //test loop - { - move(); - wait(0.5); - move(); - wait(0.5); - rotate('R'); - wait(0.5); - rotate('R'); - wait(0.5); - } - - /*while(true) - { - getCommand(); - - switch(command) - { - case 'F' : move(); //move forward - break; - case 'B' : move('B'); //move backward - break; - case 'R' : rotate('R'); //rotate right - break; - case 'L' : rotate('L'); //rotate left - } - - sendComplete(); - }*/ -} - -void DriveController::move(char direction) -{ + int diff; bool atLine = true; //tracks whether the bot is still at the starting intersection double error = 0; - if(direction == 'B') + diff=direction-orient; + + if(abs(diff)==3){ + diff/=-3; + + if(diff == 2)//back { treadDirection1 = treadDirection2 = 0; } - else + else if(direction == 0)//forward { treadDirection1 = treadDirection2 = 1; } @@ -156,22 +120,32 @@ || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]); } -void DriveController::rotate(char direction) +void DriveController::rotate(typename ::abs_direction direction) { + int diff; bool atLine = true; - if(direction == 'L') - { - treadDirection1 = 0; - treadDirection2 = 1; + diff=direction-orient + + if(abs(diff)==3){ + diff/=-3; } - else - { - treadDirection1 = 0; - treadDirection2 = 1; - } - - treadSpeed1 = treadSpeed2 = MOTOR_SCALE; + 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 { @@ -188,28 +162,6 @@ treadSpeed1 = treadSpeed2 = 0; } -void DriveController::getCommand() -{ - bool received = false; - int status; - - while(!received) - { - status = i2c.receive(); - - if(status == 2) //if status is WriteGeneral - { - command = i2c.read(); - received = true; - } - } -} - -void DriveController::sendComplete() -{ - i2c.write(1); -} - void DriveController::spinTest() { treadDirection1 = 1; @@ -220,3 +172,8 @@ while(true) wait(1); } + +typename ::abs_direction getOrient() +{ + return orient; +} \ No newline at end of file