![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
working version with calibration done
Fork of Eurobot2013 by
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
diff -r 2bd9f4e02b74 -r 5ba926692210 IR/IR.cpp
diff -r 2bd9f4e02b74 -r 5ba926692210 IR/IR.h
diff -r 2bd9f4e02b74 -r 5ba926692210 IR/PwmIn/PwmIn.cpp --- 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); - -}
diff -r 2bd9f4e02b74 -r 5ba926692210 IR/PwmIn/PwmIn.h --- 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
diff -r 2bd9f4e02b74 -r 5ba926692210 IR_Turret/IR_Turret.cpp --- /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; +}
diff -r 2bd9f4e02b74 -r 5ba926692210 IR_Turret/IR_Turret.h --- /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
diff -r 2bd9f4e02b74 -r 5ba926692210 IR_Turret/PwmIn/PwmIn.cpp --- /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); + +}
diff -r 2bd9f4e02b74 -r 5ba926692210 IR_Turret/PwmIn/PwmIn.h --- /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
diff -r 2bd9f4e02b74 -r 5ba926692210 Sonar/RFSRF05.cpp --- 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);
diff -r 2bd9f4e02b74 -r 5ba926692210 Sonar/RFSRF05.h --- 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);
diff -r 2bd9f4e02b74 -r 5ba926692210 globals.h --- 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;
diff -r 2bd9f4e02b74 -r 5ba926692210 main.cpp --- 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);
diff -r 2bd9f4e02b74 -r 5ba926692210 system/system.cpp --- /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
diff -r 2bd9f4e02b74 -r 5ba926692210 system/system.h --- /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