2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: main.cpp
- Revision:
- 12:76c9915db820
- Parent:
- 11:bbddc908c78c
- Child:
- 14:c638d4b9ee94
diff -r bbddc908c78c -r 76c9915db820 main.cpp --- a/main.cpp Fri Apr 05 17:47:06 2013 +0000 +++ b/main.cpp Sat Apr 06 14:41:35 2013 +0000 @@ -65,8 +65,7 @@ #include "Sensors/Encoders/Encoder.h" #include "Sensors/Colour/Colour.h" #include "Sensors/CakeSensor/CakeSensor.h" - - +#include <algorithm> void motortest(); void encodertest(); @@ -80,6 +79,7 @@ void ledphototransistortest(); void colourtest(); void cakesensortest(); +void feedbacktest(); int main() { @@ -95,9 +95,29 @@ //motortestline(); //ledtest(); //phototransistortest(); - ledphototransistortest(); + //ledphototransistortest(); //colourtest(); // Red SnR too low - cakesensortest(); + //cakesensortest(); + //feedbacktest(); +} + +void feedbacktest(){ + Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); + MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); + + float Pgain = -0.002; + float fwdspeed = -200/3.0f; + Timer timer; + timer.start(); + + while(true){ + float expecdist = fwdspeed * timer.read(); + float errleft = Eleft.getPoint() - (expecdist*1.07f); + float errright = Eright.getPoint() - expecdist; + + mleft(max(min(errleft*Pgain, 0.4f), -0.4f)); + mright(max(min(errright*Pgain, 0.4f), -0.4f)); + } } void cakesensortest(){ @@ -248,8 +268,8 @@ } void motorencodetestline(){ - Encoder Eleft(p27, p28), Eright(p30, p29); - MainMotor mleft(p24,p23), mright(p21,p22); + Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); + MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); //Serial pc(USBTX, USBRX); const float speed = 0.2; const float dspeed = 0.1;