Revision 8:ffc7d8af2d5a, committed 2012-04-27

Comitter:

narshu

Date:

Fri Apr 27 18:36:54 2012 +0000

Parent:

7:f9c59a3e4155

Child:

9:377560539b74

Commit message:

UI is working

Changed in this revision

--- a/Kalman/Kalman.cpp	Fri Apr 27 16:37:26 2012 +0000
+++ b/Kalman/Kalman.cpp	Fri Apr 27 18:36:54 2012 +0000
@@ -167,6 +167,9 @@
 
 void Kalman::predictloop() {
 
+    OLED4 = !ui.regid(0, 3);
+    OLED4 = !ui.regid(1, 9);
+
     float lastleft = 0;
     float lastright = 0;
 
@@ -234,12 +237,15 @@
             0, 0, varang + varangdt;
 
         P = F * P * trans(F) + Q;
+        
+        //Update UI
+        float statecpy[] = {X(0), X(1), X(2)};
+        OLED4 = !ui.updateval(0, statecpy, 3) || OLED4;
+        
+        float Pcpy[] = {P(0,0), P(0,1), P(0,2), P(1,0), P(1,1), P(1,2), P(2,0), P(2,1), P(2,2)};
+        OLED4 = !ui.updateval(1, Pcpy, 9) || OLED4;
 
         statelock.unlock();
-        //Thread::wait(PREDICTPERIOD);
-
-        //cout << "predict" << X << endl;
-        //cout << P << endl;
     }
 }
 

--- a/Kalman/Sonar/RFSRF05.cpp	Fri Apr 27 16:37:26 2012 +0000
+++ b/Kalman/Sonar/RFSRF05.cpp	Fri Apr 27 18:36:54 2012 +0000
@@ -141,7 +141,8 @@
         ValidPulse = false;
 
         //Calucate distance
-        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 300;
+        //true offset is about 330, we put 400 so circles overlap
+        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 400;
 
         if (callbackfunc)
             (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Apr 27 18:36:54 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/libraries/Servo/le3jpb
\ No newline at end of file

--- a/front_arms.h	Fri Apr 27 16:37:26 2012 +0000
+++ b/front_arms.h	Fri Apr 27 18:36:54 2012 +0000
@@ -1,15 +1,22 @@
 #ifndef _FRONT_ARMS_H
 #define _FRONT_ARMS_H
 
-#include"mbed.h"
+#include "mbed.h"
 
 #define LEFT_ARM_PIN p21
 #define RIGHT_ARM_PIN p24
 
-Ser
+Servo leftServo(LEFT_ARM_PIN);
+Servo rightServo(RIGHT_ARM_PIN);
 
-void frontArmsInit() {
-    
+void setLeftArmPosition(float degrees) {
+    if(degrees < 90) degrees = 90; // If arm moves past this angle, robot will not fit in the perimeter constraint
+    leftServo.position(degrees);
+}
+
+void setRightArmPosition(float degrees) {
+    if(degrees > 90) degrees = 90; // If arm moves past this angle, robot will not fit in the perimeter constraint
+    rightServo.position(degrees);
 }
 
 #endif // _FRONT_ARMS_H
\ No newline at end of file

--- a/ui/ui.cpp	Fri Apr 27 16:37:26 2012 +0000
+++ b/ui/ui.cpp	Fri Apr 27 18:36:54 2012 +0000
@@ -4,7 +4,7 @@
 #include "system.h"
 
 UI::UI() :
-        tUI(printtw,this,osPriorityNormal,1024) {
+        tUI(printtw,this,osPriorityNormal,2048) {
     newdataflags = 0;
     for (int i = 0; i < NUMIDS; i++) {
         idlist[i] = 0;
@@ -25,15 +25,16 @@
 }
 
 bool UI::updateval(char id, float* buffer, unsigned int 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 (int i = 0; i < length; i++)
             buffarr[id][i] = buffer[i];
         newdataflags |= (1<<id);
         return true;
-    } else
+    } else{
         return false;
+    }
 }
 
 bool UI::updateval(char id, float value) {
@@ -60,28 +61,36 @@
 
 void UI::printloop() {
 
-    OLED4 = !OLED4;
     Thread::wait(1500);
 
     char* sync = "ABCD";
     std::cout.write(sync, 4);
-    std::cout.flush();
-    //std::cout << std::endl;
+    //std::cout.flush();
+    std::cout << std::endl;
     //printf("\r\n");
-    
-    OLED4 = !OLED4;
 
     while (1) {
+    
+        OLED3 = !OLED3;
+    
+        //send number of packets
+        char numtosend = 0;
+        for (int id = 0; id < NUMIDS; id++)
+            if (newdataflags & (1<<id))
+                numtosend++;
+                
+        std::cout.put(numtosend);
 
         //send packets
-        for (int id = 0; id < NUMIDS; id++) {
+        for (char id = 0; id < NUMIDS; id++) {
             if (newdataflags & (1<<id)) {
                 std::cout.put(id);
-                std::cout.write((char*)buffarr[id], 4*idlist[id]);
+                std::cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
                 newdataflags &= ~(1<<id);
             }
         }
-
+        
+        std::cout << std::endl;
         //std::cout.flush();
         Thread::wait(200);
     }

--- a/ui/ui.h	Fri Apr 27 16:37:26 2012 +0000
+++ b/ui/ui.h	Fri Apr 27 18:36:54 2012 +0000
@@ -18,9 +18,10 @@
     bool unregid(char id);
     
 private:
+    Mutex printlock;
     char idlist[NUMIDS];
     float* buffarr[NUMIDS];
-    int newdataflags; //Only works for NUMID = 32
+    volatile int newdataflags; //Only works for NUMID = 32
     
     void printloop();
     static void printtw(void const *arg){ ((UI*)arg)->printloop(); }