Revision 5:7ac07bf30707, committed 2012-04-26

Comitter:

narshu

Date:

Thu Apr 26 23:49:49 2012 +0000

Parent:

4:7b7334441da9

Child:

6:324946320c6d

Commit message:

Fixed Sonar and most of the kalman filter except for the IR serial;

Changed in this revision

--- a/Kalman/IR/IR.cpp	Thu Apr 26 22:05:59 2012 +0000
+++ b/Kalman/IR/IR.cpp	Thu Apr 26 23:49:49 2012 +0000
@@ -3,42 +3,38 @@
 #include "system.h"
 #include "geometryfuncs.h"
 #include "globals.h"
+#include "mbed.h"
 
 IR::IR(Kalman &kalmanin):
 #ifdef ROBOT_PRIMARY
-    IRserial(p9, p10),
+        IRserial(p9, p10),
 #else
-    IRserial(p13, p14),
+        IRserial(p13, p14),
 #endif
-    kalman(kalmanin) {
-    
+        kalman(kalmanin) {
+
     //Starting values of IR calibration
     angleInit = false;
     angleOffset = 0;
-    
+
     //Setting up IR serial
     IRserial.baud(115200);
     IRserial.format(8,Serial::Odd,1);
-    }
-    
-void IR::detachisr(){
+}
+
+void IR::detachisr() {
     IRserial.attach(NULL,Serial::RxIrq);
 }
 
-void IR::attachisr(){
+void IR::attachisr() {
     IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
 }
 
 void IR::vIRValueISR (void) {
 
-    // bytes packing/unpacking for IR turret serial comm
-    union IRValue_t {
-        float IR_floats[3];
-        int IR_ints[3];
-        unsigned char IR_chars[12];
-    } IRValues;
+
 
-    
+
     // A workaround for mbed UART ISR bug
     // Clear the RBR flag to make sure the interrupt doesn't loop
     // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
@@ -49,6 +45,18 @@
     unsigned char RBR = LPC_UART1->RBR;
 #endif
 
+    //printf("%#x/n", RBR);
+
+    OLED3 = !OLED3;
+    return;
+
+    // bytes packing/unpacking for IR turret serial comm
+    static union IRValue_t {
+        float IR_floats[3];
+        int IR_ints[3];
+        unsigned char IR_chars[12];
+    } IRValues;
+
     const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
     static int Alignment_ptr = 0;
     static bool data_flag = false;
@@ -68,16 +76,22 @@
         if (buff_pointer >= 12) {
             buff_pointer = 0;
             data_flag = false; // dessert the dataflag
-            OLED3 = !OLED3;
+
+
+
+            /*
             if (angleInit) {
                 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]);
             } else {
                 //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix
                 //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
-                
+
                 //only update the IRMeasures used by kalman init
                 kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1];
             }
+            */
+
+
         }
 
     }

--- a/Kalman/IR/IR.h	Thu Apr 26 22:05:59 2012 +0000
+++ b/Kalman/IR/IR.h	Thu Apr 26 23:49:49 2012 +0000
@@ -7,22 +7,22 @@
 //forward declaration of class Kalman to avoid cyclic include
 class Kalman;
 
-class IR{
+class IR {
 public:
 
-Serial IRserial;
+    Serial IRserial;
 
-bool angleInit; // = false;
-float angleOffset; // = 0;
+    bool angleInit; // = false;
+    float angleOffset; // = 0;
 
-IR(Kalman &kalmanin);
-void detachisr();
-void attachisr();
-void vIRValueISR (void);
+    IR(Kalman &kalmanin);
+    void detachisr();
+    void attachisr();
+    void vIRValueISR (void);
 
 private:
 //reference to the kalman object to run the updates on
-Kalman& kalman;
+    Kalman& kalman;
 };
 
 #endif //IR_H
\ No newline at end of file

--- a/Kalman/Kalman.cpp	Thu Apr 26 22:05:59 2012 +0000
+++ b/Kalman/Kalman.cpp	Thu Apr 26 23:49:49 2012 +0000
@@ -98,15 +98,13 @@
 #else
     LPC_UART1->FCR = LPC_UART1->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
 #endif
-    
-    
+
     ir.attachisr();
     //wating untill the IR has reved up and picked up some data
-    Thread::wait(1000);
+    wait(1);
 
     //temporaraly disable IR updates
     ir.detachisr();
-    //IRturret.attach(NULL,Serial::RxIrq);
 
     //lock the state throughout the computation, as we will override the state at the end
     statelock.lock();

--- a/globals.h	Thu Apr 26 22:05:59 2012 +0000
+++ b/globals.h	Thu Apr 26 23:49:49 2012 +0000
@@ -7,22 +7,26 @@
 #define ROBOT_PRIMARY
 // invert echo polarity for primary
 #define SONAR_ECHO_INV
+// Primary Robot constants
+const int robot_width = 410;
+const int encoderRevCount = 1856;
+const int wheelmm = 314;
+const int robotCircumference = 1256;
+#else
+//Secondary Robot constants in mm
+const int robot_width = 260;
+const int encoderRevCount = 360;
+const int wheelmm = 226;
+const int robotCircumference = 816;
 #endif
 
 #include "mbed.h"
 
 #define PI 3.14159265
 
-//Robot constants
-//const int encoderRevCount = 1856;
-//const int wheelmm = 314;
-//const int robotCircumference = 1256;
+
 
-//Robot constants in mm
-const int robot_width = 260;
-const int encoderRevCount = 360;
-const int wheelmm = 226;
-const int robotCircumference = 816;
+
 
 //Robot movement constants
 const float fwdvarperunit = 0.005; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN!