Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: DriveController.cpp
- Revision:
- 14:7a4cf2ed2499
- Parent:
- 13:aa6f64c73271
- Child:
- 15:150ec9448efc
- Child:
- 16:b49db5c8e16c
diff -r aa6f64c73271 -r 7a4cf2ed2499 DriveController.cpp --- a/DriveController.cpp Fri Apr 03 23:02:38 2015 +0000 +++ b/DriveController.cpp Wed Apr 15 14:40:08 2015 +0000 @@ -1,7 +1,10 @@ #include "DriveController.h" -#define MOTOR_SCALE 0.4 -#define CORRECTION_SCALE 0.05 +#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, @@ -24,13 +27,13 @@ while(true) //test loop { move(); - wait(1); - rotate('R'); - wait(1); + wait(0.5); + move(); + wait(0.5); rotate('R'); - wait(1); - move(); - wait(1); + wait(0.5); + rotate('R'); + wait(0.5); } /*while(true) @@ -177,9 +180,10 @@ if(!intersection()) atLine = false; - } while(!intersection() || atLine);*/ + } + while(!intersection() || atLine);*/ - wait(0.85); + wait(0.9); treadSpeed1 = treadSpeed2 = 0; }