![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Revision 20:70d651156779, committed 2013-04-09
- Comitter:
- madcowswe
- Date:
- Tue Apr 09 15:33:36 2013 +0000
- Parent:
- 19:4b993a9a156e
- Commit message:
- Predict loop running, update loop not done.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Communication/coprocserial.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -0,0 +1,71 @@ +#include "Kalman.h" +#include "mbed.h" +#include "globals.h" + +Serial coprocserial(NC, P_SERIAL_RX); + +//DigitalOut OLED1(LED1); +DigitalOut OLED2(LED2); + +// bytes packing +typedef union { + + struct _data{ + unsigned char sync[3]; + unsigned char ID; + float value; + float variance; + } data; + + unsigned char type_char[12]; +} bytepack_t; + +bytepack_t incbuff; + +volatile int buffprintflag = 0; +void printbuff(){ + while(!buffprintflag); + buffprintflag = 0; + for(int i = 0; i < 9; i++){ + printf("%x ", incbuff.type_char[i]); + } + printf("\r\n"); +} + +void procserial(){ + + //Fetch the byte in a "clear interrupt" sense + unsigned char c = LPC_UART1->RBR; + + //OLED1 = !OLED1; + + static int ctr = 0; + + + if (ctr < 3){ + if (c == 0xFF) + ctr++; + else + ctr = 0; + } else { + incbuff.type_char[ctr] = c; + if (++ctr == 12){ + ctr = 0; + + OLED2 = !OLED2; + buffprintflag = 1; + + //runupdate + Kalman::runupdate((Kalman::measurement_t)incbuff.data.ID, incbuff.data.value, incbuff.data.variance); + } + } + +} + +void InitSerial(){ + + coprocserial.baud(115200); + + printf("attachserial\r\n"); + coprocserial.attach(procserial); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Communication/coprocserial.h Tue Apr 09 15:33:36 2013 +0000 @@ -0,0 +1,3 @@ + +void InitSerial(); +void printbuff(); \ No newline at end of file
--- a/Processes/Kalman/Kalman.cpp Sun Apr 07 19:26:07 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -5,9 +5,10 @@ #include "rtos.h" #include "math.h" #include "supportfuncs.h" +#include "Encoder.h" //#include "globals.h" -#include <tvmet/Matrix.h> +#include "tvmet/Matrix.h" using namespace tvmet; @@ -15,6 +16,11 @@ namespace Kalman { +Ticker predictticker; + +DigitalOut OLED4(LED4); +DigitalOut OLED1(LED1); + //State variables Matrix<float, 3, 1> X; Matrix<float, 3, 3> P; @@ -23,7 +29,7 @@ float RawReadings[maxmeasure+1]; float IRpahseOffset; -bool Kalman_init = 0; +bool Kalman_inited = 0; struct measurmentdata { measurement_t mtype; @@ -33,29 +39,41 @@ Mail <measurmentdata, 16> measureMQ; +Thread* predict_thread_ptr = NULL; //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void KalmanInit() { + printf("kalmaninit \r\n"); + + //WARNING: HARDCODED! + //solve for our position (assume perfect bias) - const float d = beaconpos[0].y - beaconpos[1].y; - const float i = beaconpos[0].y - beaconpos[2].y; - const float j = beaconpos[0].x - beaconpos[2].x; - float r1 = RawReadings[SONAR0]; + const float d = beaconpos[2].y - beaconpos[1].y; + const float i = beaconpos[2].y - beaconpos[0].y; + const float j = beaconpos[2].x - beaconpos[0].x; + float r1 = RawReadings[SONAR2]; float r2 = RawReadings[SONAR1]; - float r3 = RawReadings[SONAR2]; + float r3 = RawReadings[SONAR0]; + + printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3); float y_coor = (r1*r1-r2*r2+d*d)/(2*d); float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j; - + + //coordinate system hack (for now) + x_coor = beaconpos[2].x - x_coor; + y_coor = beaconpos[2].y - y_coor; + + printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor); + //IR - float IRMeasuresloc[3]; IRMeasuresloc[0] = RawReadings[IR0]; IRMeasuresloc[1] = RawReadings[IR1]; IRMeasuresloc[2] = RawReadings[IR2]; - //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); + printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); float IR_Offsets[3]; float fromb0offset = 0; @@ -66,10 +84,10 @@ //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est); - + fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]); } - + IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3); //debug @@ -80,14 +98,24 @@ X(1,0) = y_coor; X(2,0) = 0; statelock.unlock(); + + Kalman_inited = 1; } -/* -void Kalman::predictloop(void* dummy) + +State getState(){ + statelock.lock(); + State state = {X(0,0), X(1,0), X(2,0)}; + statelock.unlock(); + return state; +} + + +void predictloop(void const *dummy) { - OLED4 = !ui.regid(0, 3); - OLED4 = !ui.regid(1, 4); + //OLED4 = !ui.regid(0, 3); + //OLED4 = !ui.regid(1, 4); float lastleft = 0; float lastright = 0; @@ -96,11 +124,11 @@ Thread::signal_wait(0x1); OLED1 = !OLED1; - int leftenc = encoders.getEncoder1(); - int rightenc = encoders.getEncoder2(); + float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK; + float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK; - float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f; - float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f; + float dleft = leftenc-lastleft; + float dright = rightenc-lastright; lastleft = leftenc; lastright = rightenc; @@ -108,8 +136,8 @@ //The below calculation are in body frame (where +x is forward) float dxp, dyp,d,r; - float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f); - if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error + float thetap = (dright - dleft) / ENCODER_WHEELBASE; + if (abs(thetap) < 0.01f) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error d = (dright + dleft)/2.0f; dxp = d*cos(thetap/2.0f); dyp = d*sin(thetap/2.0f); @@ -117,20 +145,20 @@ } else { //calculate circle arc //float r = (right + left) / (4.0f * PI * thetap); r = (dright + dleft) / (2.0f*thetap); - dxp = abs(r)*sin(thetap); + dxp = r*sin(thetap); dyp = r - r*cos(thetap); } statelock.lock(); - float tempX2 = X(2); + float tempX2 = X(2,0); //rotating to cartesian frame and updating state - X(0) += dxp * cos(X(2)) - dyp * sin(X(2)); - X(1) += dxp * sin(X(2)) + dyp * cos(X(2)); - X(2) = rectifyAng(X(2) + thetap); + X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0)); + X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0)); + X(2,0) = constrainAngle(X(2,0) + thetap); //Linearising F around X - float avgX2 = (X(2) + tempX2)/2.0f; + float avgX2 = (X(2,0) + tempX2)/2.0f; Matrix<float, 3, 3> F; F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)), 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)), @@ -139,15 +167,15 @@ //Generating forward and rotational variance float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f; float varang = varperang * abs(thetap); - float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f; - float varangdt = angvarpertime * PREDICTPERIOD/1000.0f; + float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD; + float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD; //Rotating into cartesian frame Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; Qsub = varfwd + varxydt, 0, 0, varxydt; - Qrot = Rotmatrix(X(2)); + Qrot = Rotmatrix(X(2,0)); Qsubrot = Qrot * Qsub * trans(Qrot); @@ -159,24 +187,42 @@ P = F * P * trans(F) + Q; + //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0)); //Update UI - float statecpy[] = {X(0), X(1), X(2)}; - ui.updateval(0, statecpy, 3); + //float statecpy[] = {X(0,0), X(1,0), X(2,0)}; + //ui.updateval(0, statecpy, 3); - float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; - ui.updateval(1, Pcpy, 4); + //float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; + //ui.updateval(1, Pcpy, 4); statelock.unlock(); } } -void Kalman::runupdate(measurement_t type, float value, float variance) + +void predict_event_setter(){ + if(predict_thread_ptr) + predict_thread_ptr->signal_set(0x1); + else + OLED4 = 1; +} + +void start_predict_ticker(Thread* predict_thread_ptr_in){ + predict_thread_ptr = predict_thread_ptr_in; + predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD); +} + +void runupdate(measurement_t type, float value, float variance) { - if (!Kalman_init) + if (!Kalman_inited) { RawReadings[type] = value; - else { + } else { - RawReadings[type] = value - SensorOffsets[type]; + if (type >= IR0 && type <= IR2) + RawReadings[type] = value - IRpahseOffset; + else + RawReadings[type] = value; + measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { @@ -185,19 +231,20 @@ measured->variance = variance; osStatus putret = measureMQ.put(measured); - if (putret) - OLED4 = 1; + //if (putret) + //OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { - OLED4 = 1; + //OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } - + } + } - -void Kalman::updateloop(void* dummy) +/* +void Kalman::updateloop(void const *dummy) { //sonar Y chanels @@ -320,37 +367,6 @@ } -// reset kalman states -void Kalman::KalmanReset() -{ - float SonarMeasuresx1000[3]; - statelock.lock(); - SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; - SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; - SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; - //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); - - float d = beaconpos[2].y - beaconpos[1].y; - float i = beaconpos[0].y - beaconpos[1].y; - float j = beaconpos[0].x - beaconpos[1].x; - float origin_x = beaconpos[1].x; - float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); - float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; - - //statelock already locked - X(0) = x_coor/1000.0f; - X(1) = y_coor/1000.0f; - - P = 0.05, 0, 0, - 0, 0.05, 0, - 0, 0, 0.04; - - // unlocks mutexes - statelock.unlock(); - -} - */ } //Kalman Namespace -
--- a/Processes/Kalman/Kalman.h Sun Apr 07 19:26:07 2013 +0000 +++ b/Processes/Kalman/Kalman.h Tue Apr 09 15:33:36 2013 +0000 @@ -2,23 +2,25 @@ #define KALMAN_H //#include "globals.h" +#include "rtos.h" namespace Kalman { -typedef struct state { +typedef struct State { float x; float y; float theta; -} state ; +} State ; //Accessor function to get the state as one consistent struct -state getState(); +State getState(); //Main loops (to be attached as a thread in main) -void predictloop(void* dummy); -void updateloop(void* dummy); +void predictloop(void const *dummy); +void updateloop(void const *dummy); +void start_predict_ticker(Thread* predict_thread_ptr_in); enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2}; const measurement_t maxmeasure = IR2; @@ -29,13 +31,11 @@ extern float RawReadings[maxmeasure+1]; extern float IRpahseOffset; -extern bool Kalman_init; +extern bool Kalman_inited; //Initialises the kalman filter void KalmanInit(); -// reset kalman states -void KalmanReset(); } #endif //KALMAN_H \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Processes/Printing/Printing.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -0,0 +1,85 @@ +#include "Printing.h" +#ifdef PRINTINGOFF +void printingThread(void const*){Thread::wait(osWaitForever);} +bool registerID(char, size_t){return true;} +bool unregisterID(char) {return true;} +bool updateval(char, float*, size_t){return true;} +bool updateval(char id, float value){return true;} +#else +#include <iostream> +using namespace std; + +size_t idlist[NUMIDS]; // Stores length of buffer 0 => unassigned +float* buffarr[NUMIDS]; +volatile unsigned int newdataflags; + +bool registerID(char id, size_t length) { + if (id < NUMIDS && !idlist[id]) {//check if the id is already taken + idlist[id] = length; + buffarr[id] = new float[length]; + return true; + } else + return false; +} +bool unregisterID(char id) { + if (id < NUMIDS) { + idlist[id] = 0; + if (buffarr[id]) + delete buffarr[id]; + return true; + } else + return false; +} + +bool updateval(char id, float* buffer, size_t length) { + //check if the id is registered, and has buffer of correct length + if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) { + for (size_t i = 0; i < length; i++) + buffarr[id][i] = buffer[i]; + newdataflags |= (1<<id); + return true; + } else + return false; +} + +bool updateval(char id, float value){ + //check if the id is registered, and the old value has been written + if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) { + buffarr[id][0] = value; + newdataflags |= (1<<id); + return true; + } else + return false; +} + +void printingThread(void const*){ + newdataflags = 0; + for (int i = 0; i < NUMIDS; i++) { + idlist[i] = 0; + buffarr[i] = 0; + } + + + Thread::wait(3500); + while(true){ + // Send number of packets + char numtosend = 0; + for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;} + cout.put(numtosend); + + // Send packets + for (char id = 0; id < NUMIDS; id++) { + if (newdataflags & (1<<id)) { + cout.put(id); + cout.write((char*)buffarr[id], idlist[id] * sizeof(float)); + newdataflags &= ~(1<<id); + } + } + cout << endl; + Thread::wait(200); + } +} +#endif + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Encoders/Encoder.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -0,0 +1,8 @@ + +#include "globals.h" +#include "Encoder.h" + +#ifdef ENABLE_GLOBAL_ENCODERS + Encoder right_encoder(P_ENC_RIGHT_A, P_ENC_RIGHT_B); + Encoder left_encoder(P_ENC_LEFT_A, P_ENC_LEFT_B); +#endif \ No newline at end of file
--- a/Sensors/Encoders/Encoder.h Sun Apr 07 19:26:07 2013 +0000 +++ b/Sensors/Encoders/Encoder.h Tue Apr 09 15:33:36 2013 +0000 @@ -1,8 +1,10 @@ -// Eurobot13 Encoder.cpp +#ifndef ENCODER_H +#define ENCODER_H #include "QEI.h" #include "mbed.h" +#include "globals.h" class Encoder{ private: @@ -19,7 +21,7 @@ yellow.mode(PullUp); } - int getPoint(void){ + int getTicks(void){ return wheel.getPulses(); } @@ -27,3 +29,10 @@ return wheel.reset(); } }; + +#ifdef ENABLE_GLOBAL_ENCODERS + extern Encoder right_encoder; + extern Encoder left_encoder; +#endif + +#endif //ENCODER_H
--- a/Sensors/Sonar/Sonar.h Sun Apr 07 19:26:07 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,2 +0,0 @@ - -// Eurobot13 Sonar.h \ No newline at end of file
--- a/globals.h Sun Apr 07 19:26:07 2013 +0000 +++ b/globals.h Tue Apr 09 15:33:36 2013 +0000 @@ -1,4 +1,82 @@ +#ifndef GLOBALS_H +#define GLOBALS_H + +#include "mbed.h" + +const float KALMAN_PREDICT_PERIOD = 0.05; //seconds + +#define ENABLE_GLOBAL_ENCODERS + +const float ENCODER_M_PER_TICK = 0.00084; //TODO: measure this!! +const float ENCODER_WHEELBASE = 0.068; //TODO: measrue this!! +const float TURRET_FWD_PLACEMENT = 0.042; + +//Robot movement constants +const float fwdvarperunit = 0.01; //1 std dev = 7% //TODO: measrue this!! +const float varperang = 0.01; //around 1 degree stddev per 180 turn //TODO: measrue this!! +const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things +const float angvarpertime = 0.001; + +/* +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: Cake distance sensor +16: Fwd 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 + +*/ + +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; + + + +//a type which is a pointer to a rtos thread function +typedef void (*tfuncptr_t)(void const *argument); //Solving for sonar bias is done by entering the following into wolfram alpha //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f @@ -13,4 +91,6 @@ extern pos beaconpos[3]; -const float PI = 3.14159265359; \ No newline at end of file +const float PI = 3.14159265359; + +#endif //GLOBALS_H \ No newline at end of file
--- 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()); } }
--- a/supportfuncs.h Sun Apr 07 19:26:07 2013 +0000 +++ b/supportfuncs.h Tue Apr 09 15:33:36 2013 +0000 @@ -3,7 +3,7 @@ #include <cmath> #include "globals.h" - +#include "tvmet/Matrix.h" //Constrains agles to +/- pi inline float constrainAngle(float x){ @@ -13,4 +13,12 @@ return x - PI; } +template <typename T> +tvmet::Matrix <T, 2, 2> Rotmatrix(T theta) { + tvmet::Matrix <T, 2, 2> outmatrix; + outmatrix = cos(theta), -sin(theta), + sin(theta), cos(theta); + return outmatrix; +} + #endif //SUPPORTFUNCS_H \ No newline at end of file