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