2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: main.cpp
- Revision:
- 20:70d651156779
- Parent:
- 19:4b993a9a156e
- Child:
- 21:167dacfe0b14
- Child:
- 24:50805ef8c499
- Child:
- 43:c592bf6a6a2d
--- a/main.cpp Sun Apr 07 19:26:07 2013 +0000 +++ b/main.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -1,77 +1,18 @@ -//#pragma Otime // Compiler Optimisations - -// Eurobot13 main.cpp - - - -/* -PINOUT Sensors -5: RF:SDI -6 SDO -7 SCK -8 NCS -9 NIRQ -10-15 6 echo pins -16 trig -17 IRin -18-20 unused -21 stepper step -22-27 unused -28 Serial TX -29-30 unused - - -PINOUT Main -5: Lower arm servo -6: Upper arm servo - -14: Serial RX -15: Distance sensor - -20: color sensor in -21-24: Motors PWM IN 1-4 -25-26: Encoders -27-28: Encoders -29: Color sensor RED LED -30: Color sensor BLUE LED - -*/ +#include "globals.h" +#include "Kalman.h" #include "mbed.h" #include "rtos.h" -#include "globals.h" -Serial pc(USBTX, USBRX); - -const PinName P_SERVO_LOWER_ARM = p5; -const PinName P_SERVO_UPPER_ARM = p6; - -const PinName P_SERIAL_RX = p14; -const PinName P_DISTANCE_SENSOR = p15; - -const PinName P_COLOR_SENSOR_IN = p20; - -const PinName P_MOT_RIGHT_A = p21; -const PinName P_MOT_RIGHT_B = p22; -const PinName P_MOT_LEFT_A = p23; -const PinName P_MOT_LEFT_B = p24; - -const PinName P_ENC_RIGHT_A = p28; -const PinName P_ENC_RIGHT_B = p27; -const PinName P_ENC_LEFT_A = p25; -const PinName P_ENC_LEFT_B = p26; - -const PinName P_COLOR_SENSOR_RED = p29; -const PinName P_COLOR_SENSOR_BLUE = p30; - -pos beaconpos[] = {{0,0}, {0,2}, {3,1}}; - #include "Actuators/Arms/Arm.h" #include "Actuators/MainMotors/MainMotor.h" #include "Sensors/Encoders/Encoder.h" #include "Sensors/Colour/Colour.h" #include "Sensors/CakeSensor/CakeSensor.h" #include "Processes/Printing/Printing.h" +#include "coprocserial.h" #include <algorithm> +pos beaconpos[] = {{0,1}, {3,0}, {3,2}}; + void motortest(); void encodertest(); void motorencodetest(); @@ -105,9 +46,9 @@ //ledphototransistortest(); //colourtest(); // Red SnR too low //cakesensortest(); - feedbacktest(); + //feedbacktest(); - /* + /* DigitalOut l1(LED1); Thread p(printingThread, NULL, osPriorityNormal, 2048); l1=1; @@ -116,11 +57,19 @@ Thread::wait(osWaitForever); */ - //Kalman test threads - //Ticker predictticker; - //predictthread(predictloopwrapper, this, osPriorityNormal, 512) - //updatethread(updateloopwrapper, this, osPriorityNormal, 512) - //predictticker( SIGTICKARGS(predictthread, 0x1) ), + + InitSerial(); + //while(1) + // printbuff(); + wait(1); + Kalman::KalmanInit(); + + Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k + + Kalman::start_predict_ticker(&predictthread); + //Thread::wait(osWaitForever); + feedbacktest(); + } #include <cstdlib> @@ -132,7 +81,7 @@ registerID(ID,sizeof(buffer)/sizeof(buffer[0])); while (true){ for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){ - buffer[i] =ID ; + buffer[i] = ID ; } updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0])); Thread::wait(200); @@ -154,18 +103,21 @@ void feedbacktest(){ - Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); + //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.005; + Kalman::State state; + + float Pgain = -0.01; float fwdspeed = -400/3.0f; Timer timer; timer.start(); while(true){ float expecdist = fwdspeed * timer.read(); - float errleft = Eleft.getPoint() - (expecdist*1.05); - float errright = Eright.getPoint() - expecdist; + state = Kalman::getState(); + float errleft = left_encoder.getTicks() - (expecdist); + float errright = right_encoder.getTicks() - expecdist; mleft(max(min(errleft*Pgain, 0.4f), -0.4f)); mright(max(min(errright*Pgain, 0.4f), -0.4f)); @@ -174,12 +126,12 @@ void cakesensortest(){ wait(1); - pc.printf("cakesensortest"); + printf("cakesensortest"); CakeSensor cs(P_COLOR_SENSOR_IN); while(true){ wait(0.1); - pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm()); + printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm()); } } @@ -191,19 +143,19 @@ ColourEnum ce = c.getColour(); switch(ce){ case BLUE : - pc.printf("BLUE\n\r"); + printf("BLUE\n\r"); break; case RED: - pc.printf("RED\n\r"); + printf("RED\n\r"); break; case WHITE: - pc.printf("WHITE\n\r"); + printf("WHITE\n\r"); break; case INCONCLUSIVE: - pc.printf("INCONCLUSIVE\n\r"); + printf("INCONCLUSIVE\n\r"); break; default: - pc.printf("BUG\n\r"); + printf("BUG\n\r"); } } @@ -219,23 +171,23 @@ blue = 0; red = 0; for(int i = 0; i != 5; i++){ wait(0.1); - pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read()); + printf("Phototransistor Analog is (none): %f \n\r", pt.read()); } blue = 1; red = 0; for(int i = 0; i != 5; i++){ wait(0.1); - pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read()); + printf("Phototransistor Analog is (blue): %f \n\r", pt.read()); } blue = 0; red = 1; for(int i = 0; i != 5; i++){ wait(0.1); - pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read()); + printf("Phototransistor Analog is (red ): %f \n\r", pt.read()); } blue = 1; red = 1; for(int i = 0; i != 5; i++){ wait(0.1); - pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read()); + printf("Phototransistor Analog is (both): %f \n\r", pt.read()); } } } @@ -244,7 +196,7 @@ AnalogIn pt(P_COLOR_SENSOR_IN); while(true){ wait(0.1); - pc.printf("Phototransistor Analog is: %f \n\r", pt.read()); + printf("Phototransistor Analog is: %f \n\r", pt.read()); } } @@ -285,8 +237,8 @@ mleft(speed); mright(speed); servoTimer.start(); while (true){ - pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); - if (Eleft.getPoint() < Eright.getPoint()){ + printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); + if (Eleft.getTicks() < Eright.getTicks()){ mleft(speed); mright(speed - dspeed); } else { @@ -330,8 +282,8 @@ while (true){ //left 27 cm = 113 -> 0.239 cm/pulse //right 27 cm = 72 -> 0.375 cm/pulse - pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375)); - if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){ + printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375)); + if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){ mright(speed - dspeed); } else { mright(speed + dspeed); @@ -349,13 +301,13 @@ const int enc = -38; while(true){ mleft(speed); mright(0); - while(Eleft.getPoint()>enc){ - pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); + while(Eleft.getTicks()>enc){ + printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); } Eleft.reset(); Eright.reset(); mleft(0); mright(speed); - while(Eright.getPoint()>enc){ - pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); + while(Eright.getTicks()>enc){ + printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); } Eleft.reset(); Eright.reset(); } @@ -367,7 +319,7 @@ Serial pc(USBTX, USBRX); while(true){ wait(0.1); - pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint()); + printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks()); } }