Revision 11:5ba926692210, committed 2013-04-09

Comitter:

xiaxia686

Date:

Tue Apr 09 15:32:47 2013 +0000

Parent:

10:2bd9f4e02b74

Commit message:

woking version (calibrated)

Changed in this revision

--- a/IR/PwmIn/PwmIn.cpp	Sun Apr 07 16:30:49 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,76 +0,0 @@
-/* mbed PwmIn Library
- * Copyright (c) 2008-2010, sford
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "PwmIn.h"
-#include "globals.h"
-
-PwmIn::PwmIn(PinName p) : _p(p) {
-    #ifdef PWM_INVERT
-    _p.rise(this, &PwmIn::fall);
-    _p.fall(this, &PwmIn::rise);
-    #else
-    _p.rise(this, &PwmIn::rise);
-    _p.fall(this, &PwmIn::fall);
-    #endif
-
-    _period = 0.0;
-    _pulsewidth = 0.0;
-    _t.start();
-
-    //init callabck function
-    callbackfunc = NULL;
-    callbackobj = NULL;
-    mcallbackfunc = NULL;    
-
-}
-
-float PwmIn::period() {
-    return _period;
-}
-
-float PwmIn::pulsewidth() {
-    return _pulsewidth;
-}
-
-float PwmIn::dutycycle() {
-    return _pulsewidth / _period;
-}
-
-void PwmIn::rise() {
-    _period = _t.read_us();
-    _t.reset();
-    
-
-            
-            
-}
-
-void PwmIn::fall() {
-    _pulsewidth = _t.read_us();
-    
-        if (callbackfunc)
-            (*callbackfunc)(_pulsewidth);
-
-        if (callbackobj && mcallbackfunc)
-            (callbackobj->*mcallbackfunc)(_pulsewidth);    
-
-}

--- a/IR/PwmIn/PwmIn.h	Sun Apr 07 16:30:49 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,78 +0,0 @@
-/* mbed PwmIn Library
- * Copyright (c) 2008-2010, sford
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#ifndef MBED_PWMIN_H
-#define MBED_PWMIN_H
-
-#include "mbed.h"
-
-/** PwmIn class to read PWM inputs
- * 
- * Uses InterruptIn to measure the changes on the input
- * and record the time they occur
- *
- * @note uses InterruptIn, so not available on p19/p20
- */
-class DummyCT;
-
-class PwmIn {
-public:
-    /** Create a PwmIn
-     *
-     * @param p The pwm input pin (must support InterruptIn)
-     */ 
-    PwmIn(PinName p);
-    
-    /** Read the current period
-     *
-     * @returns the period in micro seconds
-     */
-    float period();
-    
-    /** Read the current pulsewidth
-     *
-     * @returns the pulsewidth in micro seconds
-     */
-    float pulsewidth();
-    
-    /** Read the current dutycycle
-     *
-     * @returns the dutycycle as a percentage, represented between 0.0-1.0
-     */
-    float dutycycle();
-    
-    /** A assigns a callback function when a new reading is available **/
-    void (*callbackfunc)(float pulsewidth);
-    DummyCT* callbackobj;
-    void (DummyCT::*mcallbackfunc)(float pulsewidth);    
-  
-
-protected:        
-    void rise();
-    void fall();
-    
-    InterruptIn _p;
-    Timer _t;
-    float _pulsewidth, _period;
-};
-
-#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR_Turret/IR_Turret.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,111 @@
+#include "IR_Turret.h"
+#include "mbed.h"
+#include "globals.h"
+#include "system.h"
+#include "geometryfuncs.h"
+
+
+IR_TURRET::IR_TURRET(PinName step_pin,
+                     PinName ir_pin)
+    : _step(step_pin),
+      _ir(ir_pin)
+{
+
+    //setup some timer counters using p30 for the hardware counter
+    //enable PCTIM2
+    LPC_SC->PCONP|=(1<<22);
+
+    //SET P30
+    LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
+
+    //configure counter
+    LPC_TIM2->TCR   =0x2;//counter disable
+    LPC_TIM2->CTCR  =0x1;//counter mode,increments on rising edges
+    LPC_TIM2->PR    =0x0;//set prescaler
+    LPC_TIM2->MR0   = STEPPER_DIV;        // Match count for 3200 counts
+    LPC_TIM2->MCR   = 2;           // Reset on Match
+    LPC_TIM2->TCR   =0x1;           // counter enable
+
+    // initiates the stepper motor
+    _step.period(STEPPER_PERIOD);
+    _step.pulsewidth(STEPPER_PERIOD/2.0f); // 50% duty cycle always
+
+    //init callabck function
+    callbackfunc = NULL;
+    callbackobj = NULL;
+    mcallbackfunc = NULL;
+
+    //init some variables
+    IR_counter = 0;
+    IR_step_counter = 0;
+    pulsewidth_max = 0;
+    step_count_old = 0;
+
+    _ir.callbackobj = (DummyCT*)this;
+    _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
+
+
+}
+
+void IR_TURRET::_ir_isr(float pulsewidth)
+{
+    static int step_count_local = 0;
+
+    step_count_local = LPC_TIM2->TC;  // save current counter value to a local
+    // resets timeout if pulse is captured
+    _ir_timeout.detach();
+
+    // 360 degrees overflow protection
+    if ( step_count_local < step_count_old ) {
+        step_count_local += STEPPER_DIV;
+    }
+
+    //IR_timeout_sema.wait();
+    step_count_old = step_count_local;
+    // finds the maximum pulsewidth
+    if ( pulsewidth > pulsewidth_max) {
+        pulsewidth_max = pulsewidth;
+    }
+    IR_counter ++;
+    IR_step_counter += step_count_local;
+
+    _ir_timeout.attach(this, &IR_TURRET::_ir_timeout_isr, IR_TIMEOUT);
+}
+
+void IR_TURRET::_ir_timeout_isr()
+{
+
+    static int IR_index = -1;
+
+    // detach the isr
+    _ir.mcallbackfunc = NULL;
+    
+    
+    if ((pulsewidth_max <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
+        IR_index = 0;
+    } else if ((pulsewidth_max <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
+        IR_index = 1;
+    } else if ((pulsewidth_max <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
+        IR_index = 2;
+    }
+
+    if (IR_index != -1) {
+        _angle[IR_index] = rectifyAng(((float)IR_step_counter/IR_counter)*STEP_ANGLE);
+        _variance[IR_index] = (STEPS_PER_PULSEPERIOD*IR_counter*STEP_ANGLE);
+
+        if (callbackfunc)
+            (*callbackfunc)(IR_index, _angle[IR_index],_variance[IR_index]);
+
+        if (callbackobj && mcallbackfunc)
+            (callbackobj->*mcallbackfunc)(IR_index, _angle[IR_index], _variance[IR_index]);
+    }
+
+    IR_counter = 0;
+    IR_step_counter = 0;
+    pulsewidth_max = 0;
+    step_count_old = 0;
+    IR_index = -1;
+
+    // reattach the callback
+    _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR_Turret/IR_Turret.h	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,81 @@
+#ifndef MBED_IR_TURRET_H
+#define MBED_IR_TURRET_H
+
+
+
+#include "mbed.h"
+#include "PwmIn.h"
+#include "globals.h"
+#include "geometryfuncs.h"
+
+// Stepper motor defines go here, p30 is reserved for the hardware counter
+#define STEPPER_PERIOD 0.0001   //Stepper motor period in seconds, 50% duty cycle
+#define STEPPER_DIV   3200    // number of steps per cycle = default * microstep
+#define STEP_ANGLE   ((float)(2*PI) / STEPPER_DIV)    // step angle of stepper
+#define IR_TIMEOUT_STEP 45 //about 5 degrees
+#define IR_TIMEOUT (STEPPER_PERIOD*IR_TIMEOUT_STEP)
+
+
+// IR Sensor defines go here
+#define PWM_INVERT // inverts the PWM for the IR sensor pwm input
+#define PULSEPERIOD_US 1500
+#define PULSEPERIOD ((float)1500/1000000)
+#define IR0_PULSEWIDTH 1000
+#define IR1_PULSEWIDTH 750
+#define IR2_PULSEWIDTH 500
+#define PULSEWIDTH_TOLERANCE 125
+#define STEPS_PER_PULSEPERIOD (PULSEPERIOD/STEPPER_PERIOD)
+
+/* SAMPLE IMPLEMENTATION!
+IR_TURRET my_ir_turret(stepper_pin,ir_pin);
+
+
+void callbinmain(int num, float dist) {
+    //Here is where you deal with your brand new reading ;D
+}
+
+int main() {
+    pc.printf("Hello World of RobotSonar!\r\n");
+    my_srf.callbackfunc = callbinmain;
+    
+    while (1);
+}
+
+ */
+ 
+class DummyCT;
+ 
+class IR_TURRET {
+public:
+
+    IR_TURRET(
+    PinName step_pin, 
+    PinName ir_pin);
+
+    
+    /** A assigns a callback function when a new reading is available **/
+    void (*callbackfunc)(int beaconnum, float angle, float variance);
+    DummyCT* callbackobj;
+    void (DummyCT::*mcallbackfunc)(int beaconnum, float angle, float variance);
+    
+    
+    
+private :
+    PwmOut _step;
+    PwmIn _ir;
+    Timeout _ir_timeout;
+    void _ir_timeout_isr (void);
+    void _ir_isr (float pulsewidth);
+    
+    int step_count_old;
+    int IR_counter;
+    int IR_step_counter;
+    float pulsewidth_max;
+    float _angle[3];
+    float _variance[3];
+    
+    
+    
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR_Turret/PwmIn/PwmIn.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,78 @@
+/* mbed PwmIn Library
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "PwmIn.h"
+#include "IR_Turret.h"
+#include "globals.h"
+
+
+PwmIn::PwmIn(PinName p) : _p(p) {
+    #ifdef PWM_INVERT
+    _p.rise(this, &PwmIn::fall);
+    _p.fall(this, &PwmIn::rise);
+    #else
+    _p.rise(this, &PwmIn::rise);
+    _p.fall(this, &PwmIn::fall);
+    #endif
+
+    _period = 0.0;
+    _pulsewidth = 0.0;
+    _t.start();
+
+    //init callabck function
+    callbackfunc = NULL;
+    callbackobj = NULL;
+    mcallbackfunc = NULL;    
+
+}
+
+float PwmIn::period() {
+    return _period;
+}
+
+float PwmIn::pulsewidth() {
+    return _pulsewidth;
+}
+
+float PwmIn::dutycycle() {
+    return _pulsewidth / _period;
+}
+
+void PwmIn::rise() {
+    _period = _t.read_us();
+    _t.reset();
+    
+
+            
+            
+}
+
+void PwmIn::fall() {
+    _pulsewidth = _t.read_us();
+    
+        if (callbackfunc)
+            (*callbackfunc)(_pulsewidth);
+
+        if (callbackobj && mcallbackfunc)
+            (callbackobj->*mcallbackfunc)(_pulsewidth);    
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR_Turret/PwmIn/PwmIn.h	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,78 @@
+/* mbed PwmIn Library
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_PWMIN_H
+#define MBED_PWMIN_H
+
+#include "mbed.h"
+
+/** PwmIn class to read PWM inputs
+ * 
+ * Uses InterruptIn to measure the changes on the input
+ * and record the time they occur
+ *
+ * @note uses InterruptIn, so not available on p19/p20
+ */
+class DummyCT;
+
+class PwmIn {
+public:
+    /** Create a PwmIn
+     *
+     * @param p The pwm input pin (must support InterruptIn)
+     */ 
+    PwmIn(PinName p);
+    
+    /** Read the current period
+     *
+     * @returns the period in micro seconds
+     */
+    float period();
+    
+    /** Read the current pulsewidth
+     *
+     * @returns the pulsewidth in micro seconds
+     */
+    float pulsewidth();
+    
+    /** Read the current dutycycle
+     *
+     * @returns the dutycycle as a percentage, represented between 0.0-1.0
+     */
+    float dutycycle();
+    
+    /** A assigns a callback function when a new reading is available **/
+    void (*callbackfunc)(float pulsewidth);
+    DummyCT* callbackobj;
+    void (DummyCT::*mcallbackfunc)(float pulsewidth);    
+  
+
+protected:        
+    void rise();
+    void fall();
+    
+    InterruptIn _p;
+    Timer _t;
+    float _pulsewidth, _period;
+};
+
+#endif

--- a/Sonar/RFSRF05.cpp	Sun Apr 07 16:30:49 2013 +0000
+++ b/Sonar/RFSRF05.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -1,4 +1,3 @@
-
 #include "RFSRF05.h"
 #include "mbed.h"
 #include "globals.h"
@@ -141,11 +140,11 @@
         ValidPulse = false;
 
         //Calucate distance
-        //true offset is about 100, we put 300 so circles overlap
-        _dist[_beacon_counter] =  _timer.read_us()/2.9 + 300;
+        //true offset is about 100, we put 300 so circles overlap, not sure about the true speed-of-sound constant
+        _dist[_beacon_counter] =  _timer.read_us()/2.8807f + 70;  //2.8807f
 
         if (callbackfunc)
-            (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
+            (*callbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
 
         if (callbackobj && mcallbackfunc)
             (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);

--- a/Sonar/RFSRF05.h	Sun Apr 07 16:30:49 2013 +0000
+++ b/Sonar/RFSRF05.h	Tue Apr 09 15:32:47 2013 +0000
@@ -1,4 +1,3 @@
-
 #ifndef MBED_RFSRF05_H
 #define MBED_RFSRF05_H
 
@@ -60,7 +59,7 @@
 
     
     /** A assigns a callback function when a new reading is available **/
-    void (*callbackfunc)(int beaconnum, float distance);
+    void (*callbackfunc)(int beaconnum, float distance, float variance);
     DummyCT* callbackobj;
     void (DummyCT::*mcallbackfunc)(int beaconnum, float distance, float variance);
     

--- a/globals.h	Sun Apr 07 16:30:49 2013 +0000
+++ b/globals.h	Tue Apr 09 15:32:47 2013 +0000
@@ -6,23 +6,13 @@
 
 #define ROBOT_PRIMARY
 
-// Stepper motor defines go here, p30 is reserved for the hardware counter
+// IR turret defines go here, p30 is reserved for the hardware counter
 #define STEPPER_PIN p21 //p21 pwmout
-#define STEPPER_PERIOD 0.0001   //Stepper motor period in seconds, 50% duty cycle
-#define STEPPER_DIV   3200    // number of steps per cycle = default * microstep
-#define STEP_ANGLE   ((float)(2*PI) / STEPPER_DIV)    // step angle of stepper
-#define IR_TIMEOUT_STEP 90 //about 10 degrees
-#define IR_TIMEOUT (STEPPER_PERIOD*IR_TIMEOUT_STEP)
+#define IR_SENSOR_PIN p17
 
-// IR Sensor defines go here
-#define PWM_INVERT // inverts the PWM for the IR sensor pwm input
-#define IR_SENSOR_PIN p17
-#define IR0_PULSEWIDTH 1000
-#define IR1_PULSEWIDTH 750
-#define IR2_PULSEWIDTH 500
-#define PULSEWIDTH_TOLERANCE 125
-
-
+// serial interface to the mainboard
+#define MBED_MAIN_SERIAL_TX p28
+#define MBED_MAIN_SERIAL_RX p27
 
 
 
@@ -33,48 +23,19 @@
 
 #ifdef ROBOT_PRIMARY
 // Primary defs go here
-//Robot hardware parameters
-const int robotCircumference = 816; //mm (DUMMY!)
 
-//Robot movement constants
-const float fwdvarperunit = 0.01; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN!
-const float varperang = 0.01; //around 1 degree stddev per 180 turn
-const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
-const float angvarpertime = 0.001;
 #else
 // Secondary defs go here
-//Robot hardware parameters
-const int robotCircumference = 816; //mm (DUMMY!)
 
-//Robot movement constants
-const float fwdvarperunit = 0.01; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN!
-const float varperang = 0.01; //around 1 degree stddev per 180 turn
-const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
-const float angvarpertime = 0.001;
 #endif
 
 
 
 //sonar constants
-static const float sonarvariance = 0.005;
+static const float sonarvariance = 0.01*0.01; //0.01m std
 
 //IR constants
-static const float IRvariance = 0.001;
-
-//Arena constants
-struct pos {
-    int x;
-    int y;
-};
-
-//beacon positions
-extern pos beaconpos[];
-
-//Colour
-extern bool Colour; // 1 for red, 0 for blue
-
-//System constants
-const int PREDICTPERIOD = 20; //ms
+static const float IRvariance = 0.0073*0.0073; //std = 0.0073
 
 //High speed serial port
 extern Serial pc;

--- a/main.cpp	Sun Apr 07 16:30:49 2013 +0000
+++ b/main.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -3,101 +3,88 @@
 #include "math.h"
 #include "globals.h"
 #include "RFSRF05.h"
+#include "IR_Turret.h"
 #include "PwmIn.h"
 #include "system.h"
 #include "geometryfuncs.h"
 
 
-// Stepper motor pwm out
-PwmOut stepper(STEPPER_PIN);
-//InterruptIn stepper_irq(STEPPER_IRQ_PIN);
-
-PwmIn ir_sensor(IR_SENSOR_PIN);
 
 Serial pc(USBTX, USBRX); // tx, rx
 
+IR_TURRET ir(STEPPER_PIN,IR_SENSOR_PIN);
 RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
+Serial mbed_serial(MBED_MAIN_SERIAL_TX,MBED_MAIN_SERIAL_RX);
+
+
+enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
+struct measurmentdata {
+    measurement_t ID;
+    float value;
+    float aux;
+} ;
+
+Mail <measurmentdata, 16> measureMQ;
+
+// bytes packing for peer to peer communication
+typedef union {
+    struct _data{
+        unsigned char header[3];
+        unsigned char ID;
+        float value;
+        float aux;
+    }  data;  
+    unsigned char type_char[sizeof(_data)];
+} bytepack_t;
+
+
+// some globals
+float sonar_dist[3];
+float IR_angles[3];
+float IR_window_sizes[3];
 
 
 
-// RTOS stuff
-Semaphore serial_sema(1);
-Semaphore IR_timeout_sema(1);
-
-// IR timeout timer
-Timeout IR_Timeout;
+void IR_Callback(int beaconnum, float angle, float aux)
+{
+    OLED1 = !OLED1;
 
-// some bad globals
-float my_dutycycle;
-float sonar_dist[3];
-int IR_counter[3] = {0,0,0};
-int IR_step_counter[3]= {0,0,0};
-int sample_count = 0;
-int step_counter;
-float IR_angles[3];
-int step_count_old = 0;
+    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
+    if (measured) {
+        measured->ID = (measurement_t)(beaconnum+3);
+        measured->value = angle;
+        measured->aux = IRvariance;
 
-void IR_Timeout_isr() {
-    OLED4 = !OLED4;
-        IR_timeout_sema.wait();
-        if ((IR_counter[0] > IR_counter[1]) && (IR_counter[0] > IR_counter[2])) {
-        IR_angles[0] = rectifyAng(((float)IR_step_counter[0]/IR_counter[0])*STEP_ANGLE);
-        } else if ((IR_counter[1] > IR_counter[0]) && (IR_counter[1] > IR_counter[2])) {
-        IR_angles[1] = rectifyAng(((float)IR_step_counter[1]/IR_counter[1])*STEP_ANGLE);
-        } else if ((IR_counter[2] > IR_counter[0]) && (IR_counter[2] > IR_counter[1])) {
-        IR_angles[2] = rectifyAng(((float)IR_step_counter[2]/IR_counter[2])*STEP_ANGLE);
-        }    
-         IR_counter[0] = 0;
-         IR_step_counter[0] = 0;   
-         IR_counter[1] = 0;
-         IR_step_counter[1] = 0;  
-         IR_counter[2] = 0;
-         IR_step_counter[2] = 0;   
-         step_count_old = 0;
-         
-         IR_timeout_sema.release();                 
-         serial_sema.release();      
+        osStatus putret = measureMQ.put(measured);
+        if (putret)
+            OLED4 = 1;
+        //    printf("putting in MQ error code %#x\r\n", putret);
+    } else {
+        OLED4 = 1;
+        //printf("MQalloc returned NULL ptr\r\n");
+    }
+
 }
 
-
-
-void IR_Callback(float pulsewidth)
-{
-    static int step_count_local;
-
-    step_count_local = LPC_TIM2->TC;  // save current counter value to a local
-    // resets timeout if pulse is captured
-    IR_Timeout.detach ();
-    
-    
-    if ( step_count_local < step_count_old ){
-        step_count_local += STEPPER_DIV;  
-    }    
-        OLED1 = !OLED1;
-        IR_timeout_sema.wait();
-        step_count_old = step_count_local;
-        if ((pulsewidth <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[0]++;
-            IR_step_counter[0] += step_count_local;
-        } else if ((pulsewidth <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[1]++;
-            IR_step_counter[1] += step_count_local;
-        } else if ((pulsewidth <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[2]++;
-            IR_step_counter[2] += step_count_local;
-        }
-        IR_timeout_sema.release();
-        
-        IR_Timeout.attach(&IR_Timeout_isr, IR_TIMEOUT);
-}
-
-
-void Sonar_Callback(int num, float dist)
+void Sonar_Callback(int num, float dist, float sonaraux)
 {
     //Here is where you deal with your brand new reading ;D
     OLED2 = !OLED2;
-    sonar_dist[num] = dist;
-    //serial_sema.release();
+
+    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
+    if (measured) {
+        measured->ID = (measurement_t)num;
+        measured->value = dist/1000.0f;
+        measured->aux = sonarvariance;
+
+        osStatus putret = measureMQ.put(measured);
+        if (putret)
+            OLED4 = 1;
+        //    printf("putting in MQ error code %#x\r\n", putret);
+    } else {
+        OLED4 = 1;
+        //printf("MQalloc returned NULL ptr\r\n");
+    }
 
 }
 
@@ -106,44 +93,61 @@
 
 void serial_thread(void const *argument)
 {
+    measurement_t type;
+    float value,aux;
+    //bytepack_t header,pack_value,pack_aux;
+    bytepack_t datapackage;
+
+    // first 3 bytes of header is used for alignment
+    datapackage.data.header[0] = 0xFF;
+    datapackage.data.header[1] = 0xFF;
+    datapackage.data.header[2] = 0xFF;
     while (true) {
-        serial_sema.wait();
-        //printf("dutycycle: %0.4f, Sonar: %0.4f, %0.4f,%0.4f \n\r", my_dutycycle, sonar_dist[0],sonar_dist[1],sonar_dist[2]);
-        printf("IR0: %f, IR1: %f, IR2: %f  \n\r",IR_angles[0],IR_angles[1],IR_angles[2]);
-        //printf("IR0: %d, IR1: %d, IR2: %d  \n\r",IR_counter[0],IR_counter[1],IR_counter[2]);
+        osEvent evt = measureMQ.get();
+
+        if (evt.status == osEventMail) {
+            OLED3 = !OLED3;
+
+            measurmentdata &measured = *(measurmentdata*)evt.value.p;
+            type = measured.ID; //Note, may support more measurment types than sonar in the future!
+            value = measured.value;
+            aux = measured.aux;
+
+            // don't forget to free the memory
+            measureMQ.free(&measured);
+            datapackage.data.ID = (unsigned char)(type);
+            
+            //if (type <= SONAR0) {
+            //     printf("SONAR   %d: %0.5f   +- %f \n",type,value*1000,aux);
+           // } else if ((type<=IR2)&&(type>=IR1)) {
+           if (type == IR0){
+                 printf("IR      %d: %0.5f     +- %f \n",type-3,value,aux);
+            }
+            
+            datapackage.data.value = value;
+            datapackage.data.aux = aux;
+
+            // output sample to main board
+            for (int i = 0; i < sizeof(datapackage); i++) {
+                mbed_serial.putc(datapackage.type_char[i]);
+               // pc.putc(datapackage.type_char[i]);
+            }          
+        }
     }
 }
 
 int main()
 {
-    pc.baud(19200);
+    pc.baud(115200);
     pc.printf("Hello from mbed\n");
 
-    IR_timeout_sema.release();
-
-    sonar.callbackfunc = Sonar_Callback;
-    ir_sensor.callbackfunc = IR_Callback;
-
-    Thread thread_serial(serial_thread);
-
-    stepper.period(STEPPER_PERIOD);
-    stepper.pulsewidth(STEPPER_PERIOD/2.0f); // servo position determined by a pulsewidth between 1-2ms
+    mbed_serial.baud(115200);
 
 
-    //some timer counters
-    //enable PCTIM2
-    LPC_SC->PCONP|=(1<<22);
-
-    //SET P30
-    LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
+    sonar.callbackfunc = Sonar_Callback;
+    ir.callbackfunc = IR_Callback;
 
-    //configure counter
-    LPC_TIM2->TCR   =0x2;//counter disable
-    LPC_TIM2->CTCR  =0x1;//counter mode,increments on rising edges
-    LPC_TIM2->PR    =0x0;//set prescaler
-    LPC_TIM2->MR0   = STEPPER_DIV;        // Match count for 3200 counts
-    LPC_TIM2->MCR   = 2;           // Reset on Match
-    LPC_TIM2->TCR   =0x1;           // counter enable
+    Thread thread_serial(serial_thread);
 
     while (true) {
         Thread::wait(osWaitForever);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system/system.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,29 @@
+#include "system.h"
+
+//Defining the externs
+DigitalOut     OLED1(LED1);
+DigitalOut     OLED2(LED2);
+DigitalOut     OLED3(LED3);
+DigitalOut     OLED4(LED4);
+
+//nop style wait function
+void nopwait(int ms){
+while(ms--)
+    for (volatile int i = 0; i < 24000; i++);
+}
+
+float cpupercent; //defining the extern
+void measureCPUidle (void const* arg) {
+    
+    Timer timer;
+    cpupercent = 0; //defined in system.h
+    
+    while(1) {
+        timer.reset();
+        timer.start();
+        wait(1);
+        
+        int thistime = timer.read_us()-1000000;
+        cpupercent = thistime;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/system/system.h	Tue Apr 09 15:32:47 2013 +0000
@@ -0,0 +1,51 @@
+
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include "globals.h"
+#include "rtos.h"
+
+//Declaring the onboard LED's for everyone to use
+extern DigitalOut     OLED1;//(LED1);
+extern DigitalOut     OLED2;//(LED2);
+extern DigitalOut     OLED3;//(LED3);
+extern DigitalOut     OLED4;//(LED4);
+
+//nop style wait function
+void nopwait(int ms);
+
+//a type which is a pointer to a rtos thread function
+typedef void (*tfuncptr_t)(void const *argument);
+
+//---------------------
+//Signal ticker stuff
+#define SIGTICKARGS(thread, signal) \
+    (tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(thread, signal))
+
+class Signalsetter {
+public:
+    Signalsetter(Thread& inthread, int insignal) :
+            thread(inthread) {
+        signal = insignal;
+        //pc.printf("ptr saved as %#x \r\n", (int)(&(inthread)));
+    }
+
+    static void callback(void* thisin) {
+
+        Signalsetter* fthis = (Signalsetter*)thisin;
+        //pc.printf("callback will signal thread object at %#x \r\n", (int)(&(fthis->thread)));
+        fthis->thread.signal_set(fthis->signal);
+        //delete fthis; //this is useful for single fire tickers!
+    }
+
+private:
+    Thread& thread;
+    int signal;
+};
+
+//---------------------
+//cpu usage measurement function
+extern float cpupercent;
+void measureCPUidle (void const* arg);
+
+#endif