Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Diff: main.cpp
- Revision:
- 14:c638d4b9ee94
- Parent:
- 12:d4b5851742a3
- Parent:
- 13:76c9915db820
- Child:
- 15:9c5aaeda36dc
diff -r d4b5851742a3 -r c638d4b9ee94 main.cpp --- a/main.cpp Fri Apr 05 21:49:23 2013 +0000 +++ b/main.cpp Sat Apr 06 15:43:47 2013 +0000 @@ -67,8 +67,7 @@ #include "Sensors/Colour/Colour.h" #include "Sensors/CakeSensor/CakeSensor.h" #include "Processes/Printing/Printing.h" - - +#include <algorithm> void motortest(); void encodertest(); @@ -84,6 +83,7 @@ void cakesensortest(); void printingtestthread(void const*); void printingtestthread2(void const*); +void feedbacktest(); int main() { @@ -102,6 +102,7 @@ //ledphototransistortest(); //colourtest(); // Red SnR too low //cakesensortest(); + //feedbacktest(); DigitalOut l1(LED1); Thread p(printingThread, NULL, osPriorityNormal, 2048); l1=1; @@ -125,6 +126,7 @@ Thread::wait(200); } } + void printingtestthread2(void const*){ const char ID = 2; float buffer[5] = {ID}; @@ -138,6 +140,26 @@ } } + +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(){ wait(1); pc.printf("cakesensortest"); @@ -286,8 +308,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;