Oskar Weigl
/
Eurobot2013
We are going to win! wohoo
Revision 6:5a52c046d8f7, committed 2012-11-14
- Comitter:
- madcowswe
- Date:
- Wed Nov 14 16:49:10 2012 +0000
- Parent:
- 5:a229f40c1210
- Child:
- 7:88753d0ad4ca
- Commit message:
- 1st Compiling of project sucessful.; Encoders stubbed.; Nothing is instansiated in main.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Encoders/encoders.cpp Wed Nov 14 16:49:10 2012 +0000 @@ -0,0 +1,23 @@ + +#include "encoders.h" +//stub implementation of encoders + +int Encoders::getEncoder1(){ + return 0; +} + +int Encoders::getEncoder2(){ + return 0; +} + +void Encoders::resetEncoders(){ + return; +} + +int Encoders::encoderToDistance(int encoder){ + return encoder; +} + +int Encoders::distanceToEncoder(int distance){ + return distance; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Encoders/encoders.h Wed Nov 14 16:49:10 2012 +0000 @@ -0,0 +1,18 @@ + +#ifndef ENCODERS_H +#define ENCODERS_H + +class Encoders { + +public: + int encoderToDistance(int encoder); + int distanceToEncoder(int distance); + + void resetEncoders(); + int getEncoder1(); + int getEncoder2(); + + +}; + +#endif //ENCODERS_H \ No newline at end of file
--- a/Kalman/Kalman.cpp Wed Nov 14 16:15:46 2012 +0000 +++ b/Kalman/Kalman.cpp Wed Nov 14 16:49:10 2012 +0000 @@ -6,7 +6,7 @@ #include "RFSRF05.h" #include "math.h" #include "globals.h" -#include "motors.h" +#include "encoders.h" #include "system.h" #include "geometryfuncs.h" @@ -14,7 +14,7 @@ #include <tvmet/Vector.h> using namespace tvmet; -Kalman::Kalman(Motors &motorsin, +Kalman::Kalman(Encoders &encodersin, UI &uiin, PinName Sonar_Trig, PinName Sonar_Echo0, @@ -41,7 +41,7 @@ Sonar_SCK, Sonar_NCS, Sonar_NIRQ), - motors(motorsin), + encoders(encodersin), ui(uiin), predictthread(predictloopwrapper, this, osPriorityNormal, 512), predictticker( SIGTICKARGS(predictthread, 0x1) ), @@ -83,7 +83,7 @@ //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void Kalman::KalmanInit() { - motors.stop(); + float SonarMeasuresx1000[3]; float IRMeasuresloc[3]; int beacon_cnt = 0; @@ -199,11 +199,11 @@ Thread::signal_wait(0x1); OLED1 = !OLED1; - int leftenc = motors.getEncoder1(); - int rightenc = motors.getEncoder2(); + int leftenc = encoders.getEncoder1(); + int rightenc = encoders.getEncoder2(); - float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; - float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f; + float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f; + float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f; lastleft = leftenc; lastright = rightenc;
--- a/Kalman/Kalman.h Wed Nov 14 16:15:46 2012 +0000 +++ b/Kalman/Kalman.h Wed Nov 14 16:49:10 2012 +0000 @@ -6,7 +6,7 @@ #include "rtos.h" //#include "Matrix.h" -//#include "motors.h" +#include "encoders.h" #include "RFSRF05.h" #include "IR.h" #include "ui.h" @@ -21,7 +21,7 @@ enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; static const measurement_t maxmeasure = IR3; - Kalman(Motors &motorsin, + Kalman(Encoders &encodersin, UI &uiin, PinName Sonar_Trig, PinName Sonar_Echo0, @@ -65,7 +65,7 @@ //Sensor interfaces RFSRF05 sonararray; - Motors& motors; + Encoders& encoders; UI& ui; Thread predictthread;
--- a/main.cpp Wed Nov 14 16:15:46 2012 +0000 +++ b/main.cpp Wed Nov 14 16:49:10 2012 +0000 @@ -1,12 +1,9 @@ #include "mbed.h" +#include "globals.h" -DigitalOut myled(LED1); +bool Colour = 1; // 1 for red, 0 for blue +pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start int main() { - while(1) { - myled = 1; - wait(0.2); - myled = 0; - wait(0.2); - } + while(1); }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/system/geometryfuncs.h Wed Nov 14 16:49:10 2012 +0000 @@ -0,0 +1,28 @@ +#ifndef GEOMETRYFUNCS_H +#define GEOMETRYFUNCS_H + +#include <tvmet/Matrix.h> + +template <typename T> +Matrix <T, 2, 2> Rotmatrix(T theta) { + Matrix <T, 2, 2> outmatrix; + outmatrix = cos(theta), -sin(theta), + sin(theta), cos(theta); + return outmatrix; +} + +// rectifies angle to range -PI to PI +template <typename T> +T rectifyAng (T ang_in) { + ang_in -= (floor(ang_in/(2*PI)))*2*PI; + if (ang_in < -PI) { + ang_in += 2*PI; + } + if (ang_in > PI) { + ang_in -= 2*PI; + } + + return ang_in; +} + +#endif //GEOMETRYFUNCS_H \ No newline at end of file