Cooper Liu
working version with calibration done
Fork of
Eurobot2013
by 
Oskar Weigl
Revision 10:2bd9f4e02b74, committed 2013-04-07
- Comitter:
- xiaxia686
- Date:
- Sun Apr 07 16:30:49 2013 +0000
- Parent:
- 9:08552997b544
- Child:
- 11:5ba926692210
- Commit message:
- Working version with no serial communication to the main board. The IR sensors uses mode for identification;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IR/PwmIn/PwmIn.cpp Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,76 @@
+/* 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);
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IR/PwmIn/PwmIn.h Sun Apr 07 16:30:49 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/Kalman/Encoders/encoders.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-
-#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
--- a/Kalman/Encoders/encoders.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,18 +0,0 @@
-
-#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/IR/IR.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,74 +0,0 @@
-#include "IR.h"
-#include "Kalman.h"
-#include "system.h"
-#include "geometryfuncs.h"
-#include "globals.h"
-#include "mbed.h"
-
-IR::IR(Kalman &kalmanin):
-#ifdef ROBOT_PRIMARY
- IRserial(p9, p10),
-#else
- IRserial(p13, p14),
-#endif
- kalman(kalmanin) {
-
- //Setting up IR serial
- IRserial.baud(115200);
- IRserial.format(8,Serial::Odd,1);
-}
-
-void IR::detachisr() {
- IRserial.attach(NULL,Serial::RxIrq);
-}
-
-void IR::attachisr() {
- IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
-}
-
-void IR::vIRValueISR (void) {
-
- // 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.
- // UART0 for USB UART
-
-#ifdef ROBOT_PRIMARY
- unsigned char RBR = LPC_UART3->RBR;
-#else
- unsigned char RBR = LPC_UART1->RBR;
-#endif
-
- // 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;
- static int buff_pointer = 0;
-
- if (!data_flag) { // look for alignment bytes
- if (RBR == Alignment_char[Alignment_ptr]) {
- Alignment_ptr ++;
- }
- if (Alignment_ptr >= 4) {
- Alignment_ptr = 0;
- data_flag = true; // set the dataflag
- }
- } else { // fetch data bytes
- IRValues.IR_chars[buff_pointer] = RBR;
- buff_pointer ++;
- if (buff_pointer >= 12) {
- buff_pointer = 0;
- data_flag = false; // dessert the dataflag
- kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRvariance);
-
-
- }
-
- }
-}
\ No newline at end of file
--- a/Kalman/IR/IR.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,25 +0,0 @@
-
-#ifndef IR_H
-#define IR_H
-
-#include "mbed.h"
-
-//forward declaration of class Kalman to avoid cyclic include
-class Kalman;
-
-class IR {
-public:
-
- Serial IRserial;
-
- IR(Kalman &kalmanin);
- void detachisr();
- void attachisr();
- void vIRValueISR (void);
-
-private:
-//reference to the kalman object to run the updates on
- Kalman& kalman;
-};
-
-#endif //IR_H
\ No newline at end of file
--- a/Kalman/Kalman.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,467 +0,0 @@
-//***************************************************************************************
-//Kalman Filter implementation
-//***************************************************************************************
-#include "Kalman.h"
-#include "rtos.h"
-#include "RFSRF05.h"
-#include "math.h"
-#include "globals.h"
-#include "encoders.h"
-#include "system.h"
-#include "geometryfuncs.h"
-
-#include <tvmet/Matrix.h>
-#include <tvmet/Vector.h>
-using namespace tvmet;
-
-Kalman::Kalman(Encoders &encodersin,
- UI &uiin,
- PinName Sonar_Trig,
- PinName Sonar_Echo0,
- PinName Sonar_Echo1,
- PinName Sonar_Echo2,
- PinName Sonar_Echo3,
- PinName Sonar_Echo4,
- PinName Sonar_Echo5,
- PinName Sonar_SDI,
- PinName Sonar_SDO,
- PinName Sonar_SCK,
- PinName Sonar_NCS,
- PinName Sonar_NIRQ) :
- ir(*this),
- sonararray(Sonar_Trig,
- Sonar_Echo0,
- Sonar_Echo1,
- Sonar_Echo2,
- Sonar_Echo3,
- Sonar_Echo4,
- Sonar_Echo5,
- Sonar_SDI,
- Sonar_SDO,
- Sonar_SCK,
- Sonar_NCS,
- Sonar_NIRQ),
- encoders(encodersin),
- ui(uiin),
- predictthread(predictloopwrapper, this, osPriorityNormal, 512),
- predictticker( SIGTICKARGS(predictthread, 0x1) ),
-// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
-// sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
- updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
-
- //Initilising offsets
- InitLock.lock();
- IR_Offset = 0;
- Sonar_Offset = 0;
- InitLock.unlock();
-
-
- //Initilising matrices
-
- // X = x, y, theta;
- if (Colour)
- X = 0.5, 0, 0;
- else
- X = 2.5, 0, PI;
-
- P = 1, 0, 0,
- 0, 1, 0,
- 0, 0, 0.04;
-
- //measurment variance R is provided by each sensor when calling runupdate
-
- //attach callback
- sonararray.callbackobj = (DummyCT*)this;
- sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
-
-
- predictticker.start(20);
-// sonarticker.start(50);
-
-}
-
-
-//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
-void Kalman::KalmanInit() {
-
- float SonarMeasuresx1000[3];
- float IRMeasuresloc[3];
- int beacon_cnt = 0;
-
-
-// doesn't work since they break the ISR
- /*
- #ifdef ROBOT_PRIMARY
- LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
- #else
- LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
- #endif
- */
- // zeros the measurements
- for (int i = 0; i < 3; i++) {
- SonarMeasures[i] = 0;
- IRMeasures[i] = 0;
- }
-
- InitLock.lock();
- //zeros offsets
- IR_Offset = 0;
- Sonar_Offset = 0;
- InitLock.unlock();
-
- // attaches ir interrup
- ir.attachisr();
-
- //wating untill the IR has reved up and picked up some valid data
- //Thread::wait(1000);
- wait(2);
-
- //temporaraly disable IR updates
- ir.detachisr();
-
- //lock the state throughout the computation, as we will override the state at the end
- InitLock.lock();
- statelock.lock();
-
-
-
- SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
- SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
- SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
- IRMeasuresloc[0] = IRMeasures[0];
- IRMeasuresloc[1] = IRMeasures[1];
- IRMeasuresloc[2] = IRMeasures[2];
- //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
-
- float d = beaconpos[2].y - beaconpos[1].y;
- float i = beaconpos[0].y - beaconpos[1].y;
- float j = beaconpos[0].x - beaconpos[1].x;
- float origin_x = beaconpos[1].x;
- float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
- float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
-
- //debug for trilateration
- printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
-
- float Dist_Exp[3];
- for (int i = 0; i < 3; i++) {
- //Compute sonar offset
- Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
- Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
-
- //Compute IR offset
- float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
- if (!Colour)
- angle_est -= PI;
- //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
- // take average offset angle from valid readings
- if (IRMeasuresloc[i] != 0) {
- beacon_cnt ++;
- // changed to current angle - estimated angle
- float angle_temp = IRMeasuresloc[i] - angle_est;
- angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
- IR_Offset += angle_temp;
- }
- }
- IR_Offset /= float(beacon_cnt);
-
- //debug
- printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
-
- //statelock already locked
- X(0) = x_coor/1000.0f;
- X(1) = y_coor/1000.0f;
-
- if (Colour)
- X(2) = 0;
- else
- X(2) = PI;
-
- // unlocks mutexes
- InitLock.unlock();
- statelock.unlock();
-
-
- //reattach the IR processing
- ir.attachisr();
-}
-
-
-void Kalman::predictloop() {
-
- OLED4 = !ui.regid(0, 3);
- OLED4 = !ui.regid(1, 4);
-
- float lastleft = 0;
- float lastright = 0;
-
- while (1) {
- Thread::signal_wait(0x1);
- OLED1 = !OLED1;
-
- int leftenc = encoders.getEncoder1();
- int rightenc = encoders.getEncoder2();
-
- float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
- float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
-
- lastleft = leftenc;
- lastright = rightenc;
-
-
- //The below calculation are in body frame (where +x is forward)
- float dxp, dyp,d,r;
- float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
- if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
- d = (dright + dleft)/2.0f;
- dxp = d*cos(thetap/2.0f);
- dyp = d*sin(thetap/2.0f);
-
- } else { //calculate circle arc
- //float r = (right + left) / (4.0f * PI * thetap);
- r = (dright + dleft) / (2.0f*thetap);
- dxp = abs(r)*sin(thetap);
- dyp = r - r*cos(thetap);
- }
-
- statelock.lock();
-
- float tempX2 = X(2);
- //rotating to cartesian frame and updating state
- X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
- X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
- X(2) = rectifyAng(X(2) + thetap);
-
- //Linearising F around X
- float avgX2 = (X(2) + tempX2)/2.0f;
- Matrix<float, 3, 3> F;
- F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
- 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
- 0, 0, 1;
-
- //Generating forward and rotational variance
- float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
- float varang = varperang * abs(thetap);
- float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
- float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
-
- //Rotating into cartesian frame
- Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
- Qsub = varfwd + varxydt, 0,
- 0, varxydt;
-
- Qrot = Rotmatrix(X(2));
-
- Qsubrot = Qrot * Qsub * trans(Qrot);
-
- //Generate Q
- Matrix<float, 3, 3> Q;//(Qsubrot);
- Q = Qsubrot(0,0), Qsubrot(0,1), 0,
- Qsubrot(1,0), Qsubrot(1,1), 0,
- 0, 0, varang + varangdt;
-
- P = F * P * trans(F) + Q;
-
- //Update UI
- float statecpy[] = {X(0), X(1), X(2)};
- ui.updateval(0, statecpy, 3);
-
- float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
- ui.updateval(1, Pcpy, 4);
-
- statelock.unlock();
- }
-}
-
-//void Kalman::sonarloop() {
-// while (1) {
-// Thread::signal_wait(0x1);
-// sonararray.startRange();
-// }
-//}
-
-
-void Kalman::runupdate(measurement_t type, float value, float variance) {
- //printf("beacon %d dist %f\r\n", sonarid, dist);
- //led2 = !led2;
-
- measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
- if (measured) {
- measured->mtype = type;
- measured->value = value;
- measured->variance = variance;
-
- 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 Kalman::updateloop() {
-
- //sonar Y chanels
- ui.regid(2, 1);
- ui.regid(3, 1);
- ui.regid(4, 1);
-
- //IR Y chanels
- ui.regid(5, 1);
- ui.regid(6, 1);
- ui.regid(7, 1);
-
- measurement_t type;
- float value,variance,rbx,rby,expecdist,Y;
- float dhdx,dhdy;
- bool aborton2stddev = false;
-
- Matrix<float, 1, 3> H;
-
- float S;
- Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
-
-
- while (1) {
- OLED2 = !OLED2;
-
- osEvent evt = measureMQ.get();
-
- if (evt.status == osEventMail) {
-
- measurmentdata &measured = *(measurmentdata*)evt.value.p;
- type = measured.mtype; //Note, may support more measurment types than sonar in the future!
- value = measured.value;
- variance = measured.variance;
-
- // don't forget to free the memory
- measureMQ.free(&measured);
-
- if (type <= maxmeasure) {
-
- if (type <= SONAR3) {
-
- InitLock.lock();
- float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
- InitLock.unlock();
-
- int sonarid = type;
- aborton2stddev = true;
-
- statelock.lock();
- //update the current sonar readings
- SonarMeasures[sonarid] = dist;
-
- rbx = X(0) - beaconpos[sonarid].x/1000.0f;
- rby = X(1) - beaconpos[sonarid].y/1000.0f;
-
- expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
- Y = dist - expecdist;
-
- //send to ui
- ui.updateval(sonarid+2, Y);
-
- dhdx = rbx / expecdist;
- dhdy = rby / expecdist;
-
- H = dhdx, dhdy, 0;
-
- } else if (type <= IR3) {
-
- aborton2stddev = false;
- int IRidx = type-3;
-
- // subtract the IR offset
- InitLock.lock();
- value -= IR_Offset;
- InitLock.unlock();
-
- statelock.lock();
- IRMeasures[IRidx] = value;
-
- rbx = X(0) - beaconpos[IRidx].x/1000.0f;
- rby = X(1) - beaconpos[IRidx].y/1000.0f;
-
- float expecang = atan2(-rby, -rbx) - X(2);
- Y = rectifyAng(value - expecang);
-
- //send to ui
- ui.updateval(IRidx + 5, Y);
-
- float dstsq = rbx*rbx + rby*rby;
- H = -rby/dstsq, rbx/dstsq, -1;
- }
-
- Matrix<float, 3, 1> PH (P * trans(H));
- S = (H * PH)(0,0) + variance;
-
- if (aborton2stddev && Y*Y > 4 * S) {
- statelock.unlock();
- continue;
- }
-
- Matrix<float, 3, 1> K (PH * (1/S));
-
- //Updating state
- X += col(K, 0) * Y;
- X(2) = rectifyAng(X(2));
-
- P = (I3 - K * H) * P;
-
- statelock.unlock();
-
- }
-
- } else {
- OLED4 = 1;
- //printf("ERROR: in updateloop, code %#x", evt);
- }
-
- }
-
-}
-
-// reset kalman states
-void Kalman::KalmanReset() {
- float SonarMeasuresx1000[3];
- statelock.lock();
- SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
- SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
- SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
- //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
-
- float d = beaconpos[2].y - beaconpos[1].y;
- float i = beaconpos[0].y - beaconpos[1].y;
- float j = beaconpos[0].x - beaconpos[1].x;
- float origin_x = beaconpos[1].x;
- float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
- float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
-
- //statelock already locked
- X(0) = x_coor/1000.0f;
- X(1) = y_coor/1000.0f;
-
-
-
-/* if (Colour){
- X(0) = 0.2;
- X(1) = 0.2;
- //X(2) = 0;
- }
- else {
- X(0) = 2.8;
- X(1) = 0.2;
- //X(2) = PI;
- }
- */
- P = 0.05, 0, 0,
- 0, 0.05, 0,
- 0, 0, 0.04;
-
- // unlocks mutexes
- statelock.unlock();
-
-}
\ No newline at end of file
--- a/Kalman/Kalman.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,100 +0,0 @@
-#ifndef KALMAN_H
-#define KALMAN_H
-
-#include "globals.h"
-
-
-#include "rtos.h"
-//#include "Matrix.h"
-#include "encoders.h"
-#include "RFSRF05.h"
-#include "IR.h"
-#include "ui.h"
-
-#include <tvmet/Matrix.h>
-#include <tvmet/Vector.h>
-using namespace tvmet;
-
-
-class Kalman {
-public:
- enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
- static const measurement_t maxmeasure = IR3;
-
- Kalman(Encoders &encodersin,
- UI &uiin,
- PinName Sonar_Trig,
- PinName Sonar_Echo0,
- PinName Sonar_Echo1,
- PinName Sonar_Echo2,
- PinName Sonar_Echo3,
- PinName Sonar_Echo4,
- PinName Sonar_Echo5,
- PinName Sonar_SDI,
- PinName Sonar_SDO,
- PinName Sonar_SCK,
- PinName Sonar_NCS,
- PinName Sonar_NIRQ);
-
- void predict();
- void runupdate(measurement_t type, float value, float variance);
-
- //State variables
- Vector<float, 3> X;
- Matrix<float, 3, 3> P;
- Mutex statelock;
-
- float SonarMeasures[3];
- float IRMeasures[3];
- float IR_Offset;
- float Sonar_Offset;
- Mutex InitLock;
-
- bool Kalman_init;
-
- //The IR is public so it's possible to print the offset in the print function
- IR ir;
-
- //Initialises the kalman filter
- void KalmanInit();
-
- // reset kalman states
- void KalmanReset();
-
-private:
-
- //Sensor interfaces
- RFSRF05 sonararray;
- Encoders& encoders;
- UI& ui;
-
- Thread predictthread;
- void predictloop();
- static void predictloopwrapper(void const *argument) {
- ((Kalman*)argument)->predictloop();
- }
- RtosTimer predictticker;
-
-// Thread sonarthread;
-// void sonarloop();
-// static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
-// RtosTimer sonarticker;
-
- struct measurmentdata {
- measurement_t mtype;
- float value;
- float variance;
- } ;
-
- Mail <measurmentdata, 16> measureMQ;
-
- Thread updatethread;
- void updateloop();
- static void updateloopwrapper(void const *argument) {
- ((Kalman*)argument)->updateloop();
- }
-
-
-};
-
-#endif //KALMAN_H
\ No newline at end of file
--- a/Kalman/Sonar/RF12B/RF12B.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,400 +0,0 @@
-#include "RF12B.h"
-
-#include "RF_defs.h"
-#include <algorithm>
-#include "system.h"
-#include "globals.h"
-
-
-RF12B::RF12B(PinName _SDI,
- PinName _SDO,
- PinName _SCK,
- PinName _NCS,
- PinName _NIRQ):spi(_SDI, _SDO, _SCK),
- NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ) {// rfled(LED3) {
-
- // SPI frequency, word lenght, polarity and phase */
- spi.format(16,0);
- spi.frequency(2000000);
-
- // Set ~CS high
- NCS = 1;
-
- // Initialise RF Module
- init();
-
- // Setup interrupt to happen on falling edge of NIRQ
- NIRQ.fall(this, &RF12B::rxISR);
-}
-
-// Returns the packet length if data is available in the receive buffer, 0 otherwise
-//unsigned int RF12B::available() {
-// return fifo.size();
-//}
-
-// Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets
-/*bool RF12B::read(unsigned char* data, unsigned int size) {
- if (fifo.size() == 0) {
- return false;
- } else {
- unsigned int i = 0;
- while (fifo.size() > 0 && i < size) {
- data[i++] = fifo.front();
- fifo.pop();
- }
- return true;
- }
-}
-*/
-
-// Reads a byte of data from the receive buffer
-/*
-unsigned char RF12B::read() {
- if (available()) {
- unsigned char data = fifo.front();
- fifo.pop();
- return data;
- } else {
- return 0xFF; // Error val although could also be data...
- }
-}
-*/
-
-// Sends a packet of data to the RF module for transmission TODO: Make asych
-void RF12B::write(unsigned char *data, unsigned char length) {
- unsigned char crc = 0;
-
- // Transmitter mode
- changeMode(TX);
-
- writeCmd(0x0000);
- send(0xAA); // PREAMBLE
- send(0xAA);
- send(0xAA);
- send(0x2D); // SYNC
- send(0xD4);
- // Packet Length
- send(length);
- crc = crc8(crc, length);
- send(crc);
- crc = crc8(crc, crc);
- // Packet Data
- for (unsigned char i=0; i<length; i++) {
- send(data[i]);
- crc = crc8(crc, data[i]);
- }
- send(crc);
- send(0xAA); // DUMMY BYTES
- send(0xAA);
- send(0xAA);
-
- // Back to receiver mode
- changeMode(RX);
- status();
-
-
-}
-
-// Transmit a 1-byte data packet
-void RF12B::write(unsigned char data) {
- write(&data, 1);
-}
-/*
-void RF12B::write(queue<char> &data, int length) {
- char crc = 0;
- char length_byte = 0;
-
- // -1 means try to transmit everything in the queue
- if (length == -1) {
- length = data.size();
- }
-
- // max length of packet is 255
- length_byte = min(length, 255);
-
- // Transmitter mode
- changeMode(TX);
-
- writeCmd(0x0000);
- send(0xAA); // PREAMBLE
- send(0xAA);
- send(0xAA);
- send(0x2D); // SYNC
- send(0xD4);
- // Packet Length
- send(length_byte);
- crc = crc8(crc, length_byte);
- send(crc);
- crc = crc8(crc, crc);
- // Packet Data
- for (char i=0; i<length_byte; i++) {
- send(data.front());
- crc = crc8(crc, data.front());
- data.pop();
- }
- send(crc);
- send(0xAA); // DUMMY BYTES
- send(0xAA);
- send(0xAA);
-
- // Back to receiver mode
- changeMode(RX);
- status();
-}
-*/
-/**********************************************************************
- * PRIVATE FUNCTIONS
- *********************************************************************/
-
-// Initialises the RF12B module
-void RF12B::init() {
- // writeCmd(0x80E7); //EL,EF,868band,12.0pF
- changeMode(RX);
- writeCmd(0xA640); //frequency select
- writeCmd(0xC647); //4.8kbps
- writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
- writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
- writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
- writeCmd(0xCED4); //SYNC=2DD4
- writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
- writeCmd(0x9850); //!mp,90kHz,MAX OUT
- writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT�CBW0
- writeCmd(0xE000); //NOT USED
- writeCmd(0xC800); //NOT USED
- writeCmd(0xC040); //1.66MHz,2.2V
-
- writeCmd(
- RFM_CONFIG_EL |
- RFM_CONFIG_EF |
- RFM_CONFIG_BAND_433 //|
- //RFM_CONFIG_X_11_0pf // meh, using default
- );
-
- // 2. Power Management Command
- // leave everything switched off for now
- /*
- writeCmd(
- RFM_POWER_MANAGEMENT // switch all off
- );
- */
-
- // 3. Frequency Setting Command
- writeCmd(
- RFM_FREQUENCY |
- RFM_FREQ_433Band(435.7) //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
- );
-
-
- // 4. Data Rate Command
- //writeCmd(RFM_DATA_RATE_9600);
- writeCmd(RFM_DATA_RATE_57600);
-
-
- // 5. Receiver Control Command
- writeCmd(
- RFM_RX_CONTROL_P20_VDI |
- RFM_RX_CONTROL_VDI_FAST |
- //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
- RFM_RX_CONTROL_BW_134 | // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
- RFM_RX_CONTROL_GAIN_0 |
- RFM_RX_CONTROL_RSSI_103 // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
- );
-
- // 6. Data Filter Command
- writeCmd(
- RFM_DATA_FILTER_AL |
- RFM_DATA_FILTER_ML |
- RFM_DATA_FILTER_DIG //|
- //RFM_DATA_FILTER_DQD(4)
- );
-
- // 7. FIFO and Reset Mode Command
- writeCmd(
- RFM_FIFO_IT(8) |
- RFM_FIFO_DR |
- 0x8 //turn on 16bit sync word
- );
-
- // 8. FIFO Syncword
- // Leave as default: 0xD4
-
- // 9. Receiver FIFO Read
- // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
- // result = RFM_READ_FIFO();
-
- // 10. AFC Command
- writeCmd(
- //RFM_AFC_AUTO_VDI | //Note this might be changed to improve range. Refer to datasheet.
- RFM_AFC_AUTO_INDEPENDENT |
- RFM_AFC_RANGE_LIMIT_7_8 |
- RFM_AFC_EN |
- RFM_AFC_OE |
- RFM_AFC_FI
- );
-
- // 11. TX Configuration Control Command
- writeCmd(
- RFM_TX_CONTROL_MOD_60 |
- RFM_TX_CONTROL_POW_0
- );
-
-
- // 12. PLL Setting Command
- writeCmd(
- 0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
- // I think this will slow down the pll's reaction time. Not sure, check with someone!
- );
-
- changeMode(RX);
- resetRX();
- status();
-}
-
-/* Write a command to the RF Module */
-unsigned int RF12B::writeCmd(unsigned int cmd) {
- NCS = 0;
- unsigned int recv = spi.write(cmd);
- NCS = 1;
- return recv;
-}
-
-/* Sends a byte of data across RF */
-void RF12B::send(unsigned char data) {
- while (NIRQ);
- writeCmd(0xB800 + data);
-}
-
-/* Change the mode of the RF module to Transmitting or Receiving */
-void RF12B::changeMode(rfmode_t _mode) {
- mode = _mode;
- if (_mode == TX) {
- writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
- } else { /* mode == RX */
- writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
- }
-}
-
-// Interrupt routine for data reception */
-void RF12B::rxISR() {
-
- unsigned int data = 0;
- static int i = -2;
- static unsigned char packet_length = 0;
- static unsigned char crc = 0;
-// #ifdef ROBOT_SECONDARY
- static unsigned char temp;
-// #endif
-
- //Loop while interrupt is asserted
- while (!NIRQ_in && mode == RX) {
-
- // Grab the packet's length byte
- if (i == -2) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- packet_length = (data&0x00FF);
- crc = crc8(crc, packet_length);
- i++;
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- // Check that packet length was correct
- if (i == -1) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- unsigned char crcofsize = (data&0x00FF);
- if (crcofsize != crc) {
- //It was wrong, start over
- i = -2;
- packet_length = 0;
- crc = 0;
- //temp = queue<unsigned char>();
- resetRX();
- } else {
- crc = crc8(crc, crcofsize);
- i++;
- }
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- // Grab the packet's data
- if (i >= 0 && i < packet_length) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- // #ifdef ROBOT_SECONDARY
- temp = data&0x00FF;
- // #endif
- //temp.push(data&0x00FF);
- crc = crc8(crc, (unsigned char)(data&0x00FF));
- i++;
- }
- }
-
- //If we exhaust the interrupt, exit
- if (NIRQ_in)
- break;
-
- if (i >= packet_length) {
- data = writeCmd(0x0000);
- if ( (data&0x8000) ) {
- data = writeCmd(0xB000);
- if ((unsigned char)(data & 0x00FF) == crc) {
- //If the checksum is correct, add our data to the end of the output buffer
- //while (!temp.empty()) {
- //fifo.push(temp);
- // temp.pop();
-//#ifdef ROBOT_SECONDARY
- if (callbackfunc)
- (*callbackfunc)(temp);
-
- if (callbackobj && mcallbackfunc)
- (callbackobj->*mcallbackfunc)(temp);
-//#endif
- // }
- }
-
- // Tell RF Module we are finished, and clean up
- i = -2;
- packet_length = 0;
- crc = 0;
- //temp = queue<unsigned char>();
- resetRX();
- }
- }
- }
-
-}
-
-unsigned int RF12B::status() {
- return writeCmd(0x0000);
-}
-
-// Tell the RF Module this packet is received and wait for the next */
-void RF12B::resetRX() {
- writeCmd(0xCA81);
- writeCmd(0xCA83);
-};
-
-// Calculate CRC8 */
-unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
- crc = crc ^ data;
- for (int i = 0; i < 8; i++) {
- if (crc & 0x01) {
- crc = (crc >> 1) ^ 0x8C;
- } else {
- crc >>= 1;
- }
- }
- return crc;
-}
\ No newline at end of file
--- a/Kalman/Sonar/RF12B/RF12B.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,83 +0,0 @@
-#ifndef _RF12B_H
-#define _RF12B_H
-
-#include "mbed.h"
-//#include <queue>
-
-enum rfmode_t{RX, TX};
-
-class DummyCT;
-
-class RF12B {
-public:
- /* Constructor */
- RF12B(PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ);
-
-
-
- /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
- bool read(unsigned char* data, unsigned int size);
-
- /* Reads a byte of data from the receive buffer
- Returns 0xFF if there is no data */
- unsigned char read();
-
- /* Transmits a packet of data */
- void write(unsigned char* data, unsigned char length);
- void write(unsigned char data); /* 1-byte packet */
-// void write(std::queue<char> &data, int length = -1); /* sends a whole queue */
-
- /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
- unsigned int available();
-
- /** A assigns a callback function when a new reading is available **/
- void (*callbackfunc)(unsigned char rx_code);
- DummyCT* callbackobj;
- void (DummyCT::*mcallbackfunc)(unsigned char rx_code);
-
-protected:
- /* Receive FIFO buffer */
-// std::queue<unsigned char> fifo;
-// std::queue<unsigned char> temp; //for storing stuff mid-packet
-
- /* SPI module */
- SPI spi;
-
- /* Other digital pins */
- DigitalOut NCS;
- InterruptIn NIRQ;
- DigitalIn NIRQ_in;
- //DigitalOut rfled;
-
- rfmode_t mode;
-
- /* Initialises the RF12B module */
- void init();
-
- /* Write a command to the RF Module */
- unsigned int writeCmd(unsigned int cmd);
-
- /* Sends a byte of data across RF */
- void send(unsigned char data);
-
- /* Switch module between receive and transmit modes */
- void changeMode(rfmode_t mode);
-
- /* Interrupt routine for data reception */
- void rxISR();
-
- /* Tell the RF Module this packet is received and wait for the next */
- void resetRX();
-
- /* Return the RF Module Status word */
- unsigned int status();
-
- /* Calculate CRC8 */
- unsigned char crc8(unsigned char crc, unsigned char data);
-};
-
-#endif /* _RF12B_H */
\ No newline at end of file
--- a/Kalman/Sonar/RF12B/RF_defs.h Wed Nov 14 17:15:53 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,478 +0,0 @@ -/* - * Open HR20 - * - * target: ATmega169 @ 4 MHz in Honnywell Rondostat HR20E - * - * compiler: WinAVR-20071221 - * avr-libc 1.6.0 - * GCC 4.2.2 - * - * copyright: 2008 Dario Carluccio (hr20-at-carluccio-dot-de) - * 2008 Jiri Dobry (jdobry-at-centrum-dot-cz) - * 2008 Mario Fischer (MarioFischer-at-gmx-dot-net) - * 2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net) - * - * license: This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU Library General Public - * License as published by the Free Software Foundation; either - * version 2 of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http:*www.gnu.org/licenses - */ - -/* - * \file rfm.h - * \brief functions to control the RFM12 Radio Transceiver Module - * \author Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net> - * \date $Date: 2010/04/17 17:57:02 $ - * $Rev: 260 $ - */ - - -//#pragma once // multi-iclude prevention. gcc knows this pragma -#ifndef rfm_H -#define rfm_H - - -#define RFM_SPI_16(OUTVAL) rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value) - -#define RFM_CLK_OUTPUT 0 - -/* -#define RFM_TESTPIN_INIT -#define RFM_TESTPIN_ON -#define RFM_TESTPIN_OFF -#define RFM_TESTPIN_TOG - -#define RFM_CONFIG_DISABLE 0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do -#define RFM_CONFIG_BROADCASTSTATUS 0x01 //<! Flag that enables the HR20's status broadcast every minute - -#define RFM_CONFIG_ENABLEALL 0xff -*/ - - -/////////////////////////////////////////////////////////////////////////////// -// -// RFM status bits -// -/////////////////////////////////////////////////////////////////////////////// - -// Interrupt bits, latched //////////////////////////////////////////////////// - -#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits - // Cleared by any FIFO read method - -#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive - // Cleared by TX write - -#define RFM_STATUS_POR 0x4000 // Power On reset - // Cleared by read status - -#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write - // Cleared by read status - -#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow - // Cleared by read status - -#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow - // Cleared by read status - -#define RFM_STATUS_EXT 0x0800 // Interupt changed to low - // Cleared by read status - -#define RFM_STATUS_LBD 0x0400 // Low battery detect - -// Status bits //////////////////////////////////////////////////////////////// - -#define RFM_STATUS_FFEM 0x0200 // FIFO is empty -#define RFM_STATUS_ATS 0x0100 // TX mode: Strong enough RF signal -#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit -#define RFM_STATUS_DQD 0x0080 // Data Quality detector output -#define RFM_STATUS_CRL 0x0040 // Clock recovery lock -#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle - -/////////////////////////////////////////////////////////////////////////////// -// -// 1. Configuration Setting Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_CONFIG 0x8000 - -#define RFM_CONFIG_EL 0x8080 // Enable TX Register -#define RFM_CONFIG_EF 0x8040 // Enable RX FIFO buffer -#define RFM_CONFIG_BAND_315 0x8000 // Frequency band -#define RFM_CONFIG_BAND_433 0x8010 -#define RFM_CONFIG_BAND_868 0x8020 -#define RFM_CONFIG_BAND_915 0x8030 -#define RFM_CONFIG_X_8_5pf 0x8000 // Crystal Load Capacitor -#define RFM_CONFIG_X_9_0pf 0x8001 -#define RFM_CONFIG_X_9_5pf 0x8002 -#define RFM_CONFIG_X_10_0pf 0x8003 -#define RFM_CONFIG_X_10_5pf 0x8004 -#define RFM_CONFIG_X_11_0pf 0x8005 -#define RFM_CONFIG_X_11_5pf 0x8006 -#define RFM_CONFIG_X_12_0pf 0x8007 -#define RFM_CONFIG_X_12_5pf 0x8008 -#define RFM_CONFIG_X_13_0pf 0x8009 -#define RFM_CONFIG_X_13_5pf 0x800A -#define RFM_CONFIG_X_14_0pf 0x800B -#define RFM_CONFIG_X_14_5pf 0x800C -#define RFM_CONFIG_X_15_0pf 0x800D -#define RFM_CONFIG_X_15_5pf 0x800E -#define RFM_CONFIG_X_16_0pf 0x800F - -/////////////////////////////////////////////////////////////////////////////// -// -// 2. Power Management Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_POWER_MANAGEMENT 0x8200 - -#define RFM_POWER_MANAGEMENT_ER 0x8280 // Enable receiver -#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block -#define RFM_POWER_MANAGEMENT_ET 0x8220 // Enable transmitter -#define RFM_POWER_MANAGEMENT_ES 0x8210 // Enable synthesizer -#define RFM_POWER_MANAGEMENT_EX 0x8208 // Enable crystal oscillator -#define RFM_POWER_MANAGEMENT_EB 0x8204 // Enable low battery detector -#define RFM_POWER_MANAGEMENT_EW 0x8202 // Enable wake-up timer -#define RFM_POWER_MANAGEMENT_DC 0x8201 // Disable clock output of CLK pin - -#ifndef RFM_CLK_OUTPUT - #error RFM_CLK_OUTPUT must be defined to 0 or 1 -#endif -#if RFM_CLK_OUTPUT - #define RFM_TX_ON_PRE() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_TX_ON() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_ET | \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_RX_ON() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_ER | \ - RFM_POWER_MANAGEMENT_EBB | \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_OFF() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_EX ) -#else - #define RFM_TX_ON_PRE() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_DC | \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_TX_ON() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_DC | \ - RFM_POWER_MANAGEMENT_ET | \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_RX_ON() RFM_SPI_16( \ - RFM_POWER_MANAGEMENT_DC | \ - RFM_POWER_MANAGEMENT_ER | \ - RFM_POWER_MANAGEMENT_EBB | \ - RFM_POWER_MANAGEMENT_ES | \ - RFM_POWER_MANAGEMENT_EX ) - #define RFM_OFF() RFM_SPI_16(RFM_POWER_MANAGEMENT_DC) -#endif -/////////////////////////////////////////////////////////////////////////////// -// -// 3. Frequency Setting Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_FREQUENCY 0xA000 - -#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000) -#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000) -#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000) -#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000) - -/////////////////////////////////////////////////////////////////////////////// -// -// 4. Data Rate Command -// -///////////////////////////////////////////////////////////////////////////////// - -#define RFM_BAUD_RATE 9600 - -#define RFM_DATA_RATE 0xC600 - -#define RFM_DATA_RATE_CS 0xC680 -#define RFM_DATA_RATE_4800 0xC647 -#define RFM_DATA_RATE_9600 0xC623 -#define RFM_DATA_RATE_19200 0xC611 -#define RFM_DATA_RATE_38400 0xC608 -#define RFM_DATA_RATE_57600 0xC605 - -#define RFM_SET_DATARATE(baud) ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) ) - -/////////////////////////////////////////////////////////////////////////////// -// -// 5. Receiver Control Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_RX_CONTROL 0x9000 - -#define RFM_RX_CONTROL_P20_INT 0x9000 // Pin20 = ExternalInt -#define RFM_RX_CONTROL_P20_VDI 0x9400 // Pin20 = VDI out - -#define RFM_RX_CONTROL_VDI_FAST 0x9000 // fast VDI Response time -#define RFM_RX_CONTROL_VDI_MED 0x9100 // medium -#define RFM_RX_CONTROL_VDI_SLOW 0x9200 // slow -#define RFM_RX_CONTROL_VDI_ON 0x9300 // Always on - -#define RFM_RX_CONTROL_BW_400 0x9020 // bandwidth 400kHz -#define RFM_RX_CONTROL_BW_340 0x9040 // bandwidth 340kHz -#define RFM_RX_CONTROL_BW_270 0x9060 // bandwidth 270kHz -#define RFM_RX_CONTROL_BW_200 0x9080 // bandwidth 200kHz -#define RFM_RX_CONTROL_BW_134 0x90A0 // bandwidth 134kHz -#define RFM_RX_CONTROL_BW_67 0x90C0 // bandwidth 67kHz - -#define RFM_RX_CONTROL_GAIN_0 0x9000 // LNA gain 0db -#define RFM_RX_CONTROL_GAIN_6 0x9008 // LNA gain -6db -#define RFM_RX_CONTROL_GAIN_14 0x9010 // LNA gain -14db -#define RFM_RX_CONTROL_GAIN_20 0x9018 // LNA gain -20db - -#define RFM_RX_CONTROL_RSSI_103 0x9000 // DRSSI threshold -103dbm -#define RFM_RX_CONTROL_RSSI_97 0x9001 // DRSSI threshold -97dbm -#define RFM_RX_CONTROL_RSSI_91 0x9002 // DRSSI threshold -91dbm -#define RFM_RX_CONTROL_RSSI_85 0x9003 // DRSSI threshold -85dbm -#define RFM_RX_CONTROL_RSSI_79 0x9004 // DRSSI threshold -79dbm -#define RFM_RX_CONTROL_RSSI_73 0x9005 // DRSSI threshold -73dbm -//#define RFM_RX_CONTROL_RSSI_67 0x9006 // DRSSI threshold -67dbm // RF12B reserved -//#define RFM_RX_CONTROL_RSSI_61 0x9007 // DRSSI threshold -61dbm // RF12B reserved - -#define RFM_RX_CONTROL_BW(baud) (((baud)<8000) ? \ - RFM_RX_CONTROL_BW_67 : \ - ( \ - ((baud)<30000) ? \ - RFM_RX_CONTROL_BW_134 : \ - RFM_RX_CONTROL_BW_200 \ - )) - -/////////////////////////////////////////////////////////////////////////////// -// -// 6. Data Filter Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_DATA_FILTER 0xC228 - -#define RFM_DATA_FILTER_AL 0xC2A8 // clock recovery auto-lock -#define RFM_DATA_FILTER_ML 0xC268 // clock recovery fast mode -#define RFM_DATA_FILTER_DIG 0xC228 // data filter type digital -#define RFM_DATA_FILTER_ANALOG 0xC238 // data filter type analog -#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7)) - -/////////////////////////////////////////////////////////////////////////////// -// -// 7. FIFO and Reset Mode Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_FIFO 0xCA00 - -#define RFM_FIFO_AL 0xCA04 // FIFO Start condition sync-word/always -#define RFM_FIFO_FF 0xCA02 // Enable FIFO fill -#define RFM_FIFO_DR 0xCA01 // Disable hi sens reset mode -#define RFM_FIFO_IT(level) (RFM_FIFO | (( (level) & 0xF)<<4)) - -#define RFM_FIFO_OFF() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_DR) -#define RFM_FIFO_ON() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR) - -///////////////////////////////////////////////////////////////////////////// -// -// 8. Receiver FIFO Read -// -///////////////////////////////////////////////////////////////////////////// - -#define RFM_READ_FIFO() (RFM_SPI_16(0xB000) & 0xFF) - -///////////////////////////////////////////////////////////////////////////// -// -// 9. AFC Command -// -///////////////////////////////////////////////////////////////////////////// - -#define RFM_AFC 0xC400 - -#define RFM_AFC_EN 0xC401 -#define RFM_AFC_OE 0xC402 -#define RFM_AFC_FI 0xC404 -#define RFM_AFC_ST 0xC408 - -// Limits the value of the frequency offset register to the next values: - -#define RFM_AFC_RANGE_LIMIT_NO 0xC400 // 0: No restriction -#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres -#define RFM_AFC_RANGE_LIMIT_7_8 0xC420 // 2: +7 fres to -8 fres -#define RFM_AFC_RANGE_LIMIT_3_4 0xC430 // 3: +3 fres to -4 fres - -// fres=2.5 kHz in 315MHz and 433MHz Bands -// fres=5.0 kHz in 868MHz Band -// fres=7.5 kHz in 915MHz Band - -#define RFM_AFC_AUTO_OFF 0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller) -#define RFM_AFC_AUTO_ONCE 0xC440 // 1: Runs only once after each power-up -#define RFM_AFC_AUTO_VDI 0xC480 // 2: Keep the foffset only during receiving(VDI=high) -#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal - -/////////////////////////////////////////////////////////////////////////////// -// -// 10. TX Configuration Control Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_TX_CONTROL 0x9800 - -#define RFM_TX_CONTROL_POW_0 0x9800 -#define RFM_TX_CONTROL_POW_3 0x9801 -#define RFM_TX_CONTROL_POW_6 0x9802 -#define RFM_TX_CONTROL_POW_9 0x9803 -#define RFM_TX_CONTROL_POW_12 0x9804 -#define RFM_TX_CONTROL_POW_15 0x9805 -#define RFM_TX_CONTROL_POW_18 0x9806 -#define RFM_TX_CONTROL_POW_21 0x9807 -#define RFM_TX_CONTROL_MOD_15 0x9800 -#define RFM_TX_CONTROL_MOD_30 0x9810 -#define RFM_TX_CONTROL_MOD_45 0x9820 -#define RFM_TX_CONTROL_MOD_60 0x9830 -#define RFM_TX_CONTROL_MOD_75 0x9840 -#define RFM_TX_CONTROL_MOD_90 0x9850 -#define RFM_TX_CONTROL_MOD_105 0x9860 -#define RFM_TX_CONTROL_MOD_120 0x9870 -#define RFM_TX_CONTROL_MOD_135 0x9880 -#define RFM_TX_CONTROL_MOD_150 0x9890 -#define RFM_TX_CONTROL_MOD_165 0x98A0 -#define RFM_TX_CONTROL_MOD_180 0x98B0 -#define RFM_TX_CONTROL_MOD_195 0x98C0 -#define RFM_TX_CONTROL_MOD_210 0x98D0 -#define RFM_TX_CONTROL_MOD_225 0x98E0 -#define RFM_TX_CONTROL_MOD_240 0x98F0 -#define RFM_TX_CONTROL_MP 0x9900 - -#define RFM_TX_CONTROL_MOD(baud) (((baud)<8000) ? \ - RFM_TX_CONTROL_MOD_45 : \ - ( \ - ((baud)<20000) ? \ - RFM_TX_CONTROL_MOD_60 : \ - ( \ - ((baud)<30000) ? \ - RFM_TX_CONTROL_MOD_75 : \ - ( \ - ((baud)<40000) ? \ - RFM_TX_CONTROL_MOD_90 : \ - RFM_TX_CONTROL_MOD_120 \ - ) \ - ) \ - )) - -///////////////////////////////////////////////////////////////////////////// -// -// 11. Transmitter Register Write Command -// -///////////////////////////////////////////////////////////////////////////// - -//#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | ((byte) & 0xFF)) -#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | (byte) ) - -/////////////////////////////////////////////////////////////////////////////// -// -// 12. Wake-up Timer Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_WAKEUP_TIMER 0xE000 -#define RFM_WAKEUP_SET(time) RFM_SPI_16(RFM_WAKEUP_TIMER | (time)) - -#define RFM_WAKEUP_480s (RFM_WAKEUP_TIMER |(11 << 8)| 234) -#define RFM_WAKEUP_240s (RFM_WAKEUP_TIMER |(10 << 8)| 234) -#define RFM_WAKEUP_120s (RFM_WAKEUP_TIMER |(9 << 8)| 234) -#define RFM_WAKEUP_119s (RFM_WAKEUP_TIMER |(9 << 8)| 232) - -#define RFM_WAKEUP_60s (RFM_WAKEUP_TIMER |(8 << 8) | 235) -#define RFM_WAKEUP_59s (RFM_WAKEUP_TIMER |(8 << 8) | 230) - -#define RFM_WAKEUP_30s (RFM_WAKEUP_TIMER |(7 << 8) | 235) -#define RFM_WAKEUP_29s (RFM_WAKEUP_TIMER |(7 << 8) | 227) - -#define RFM_WAKEUP_8s (RFM_WAKEUP_TIMER |(5 << 8) | 250) -#define RFM_WAKEUP_7s (RFM_WAKEUP_TIMER |(5 << 8) | 219) -#define RFM_WAKEUP_6s (RFM_WAKEUP_TIMER |(6 << 8) | 94) -#define RFM_WAKEUP_5s (RFM_WAKEUP_TIMER |(5 << 8) | 156) -#define RFM_WAKEUP_4s (RFM_WAKEUP_TIMER |(5 << 8) | 125) -#define RFM_WAKEUP_1s (RFM_WAKEUP_TIMER |(2 << 8) | 250) -#define RFM_WAKEUP_900ms (RFM_WAKEUP_TIMER |(2 << 8) | 225) -#define RFM_WAKEUP_800ms (RFM_WAKEUP_TIMER |(2 << 8) | 200) -#define RFM_WAKEUP_700ms (RFM_WAKEUP_TIMER |(2 << 8) | 175) -#define RFM_WAKEUP_600ms (RFM_WAKEUP_TIMER |(2 << 8) | 150) -#define RFM_WAKEUP_500ms (RFM_WAKEUP_TIMER |(2 << 8) | 125) -#define RFM_WAKEUP_400ms (RFM_WAKEUP_TIMER |(2 << 8) | 100) -#define RFM_WAKEUP_300ms (RFM_WAKEUP_TIMER |(2 << 8) | 75) -#define RFM_WAKEUP_200ms (RFM_WAKEUP_TIMER |(2 << 8) | 50) -#define RFM_WAKEUP_100ms (RFM_WAKEUP_TIMER |(2 << 8) | 25) - -/////////////////////////////////////////////////////////////////////////////// -// -// 13. Low Duty-Cycle Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_LOW_DUTY_CYCLE 0xC800 - -/////////////////////////////////////////////////////////////////////////////// -// -// 14. Low Battery Detector Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_LOW_BATT_DETECT 0xC000 -#define RFM_LOW_BATT_DETECT_D_1MHZ 0xC000 -#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020 -#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040 -#define RFM_LOW_BATT_DETECT_D_2MHZ 0xC060 -#define RFM_LOW_BATT_DETECT_D_2_5MHZ 0xC080 -#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0 -#define RFM_LOW_BATT_DETECT_D_5MHZ 0xC0C0 -#define RFM_LOW_BATT_DETECT_D_10MHZ 0xC0E0 - -/////////////////////////////////////////////////////////////////////////////// -// -// 15. Status Read Command -// -/////////////////////////////////////////////////////////////////////////////// - -#define RFM_READ_STATUS() RFM_SPI_16(0x0000) -#define RFM_READ_STATUS_FFIT() SPI_1 (0x00) -#define RFM_READ_STATUS_RGIT RFM_READ_STATUS_FFIT - -/////////////////////////////////////////////////////////////////////////////// - -// RFM air protocol flags: - -#define RFMPROTO_FLAGS_BITASK_PACKETTYPE 0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype -#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST 0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1) -#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND 0b01000000 //!< command packettype (message to hr20, protocol; step 2) -#define RFMPROTO_FLAGS_PACKETTYPE_REPLY 0b10000000 //!< reply packettype (message from hr20, protocol; step 3) -#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL 0b11000000 //!< currently unused packettype - -#define RFMPROTO_FLAGS_BITASK_DEVICETYPE 0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist -#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 0b00010100 //!< topen HR20 device type. 10100 is for decimal 20 - -#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) -#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) -#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_REPLY == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) -#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) -#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS) ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) ) - -/////////////////////////////////////////////////////////////////////////////// - -#endif \ No newline at end of file
--- a/Kalman/Sonar/RFSRF05.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,183 +0,0 @@
-
-#include "RFSRF05.h"
-#include "mbed.h"
-#include "globals.h"
-#include "system.h"
-
-
-RFSRF05::RFSRF05(PinName trigger,
- PinName echo0,
- PinName echo1,
- PinName echo2,
- PinName echo3,
- PinName echo4,
- PinName echo5,
- PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ)
- : _rf(SDI,SDO,SCK,NCS,NIRQ),
- _trigger(trigger),
- _echo0(echo0),
- _echo1(echo1),
- _echo2(echo2),
- _echo3(echo3),
- _echo4(echo4),
- _echo5(echo5) {
-
- // initialises codes
- codes[0] = CODE0;
- codes[1] = CODE1;
- codes[2] = CODE2;
-
- //set callback execute to true
- ValidPulse = false;
-
- // Attach interrupts
-#ifdef SONAR_ECHO_INV
- // inverted sonar inputs
- _echo5.fall(this, &RFSRF05::_rising);
- _echo0.rise(this, &RFSRF05::_falling);
- _echo1.rise(this, &RFSRF05::_falling);
- _echo2.rise(this, &RFSRF05::_falling);
- _echo3.rise(this, &RFSRF05::_falling);
- _echo4.rise(this, &RFSRF05::_falling);
- _echo5.rise(this, &RFSRF05::_falling);
-#else
- _echo5.rise(this, &RFSRF05::_rising);
- _echo0.fall(this, &RFSRF05::_falling);
- _echo1.fall(this, &RFSRF05::_falling);
- _echo2.fall(this, &RFSRF05::_falling);
- _echo3.fall(this, &RFSRF05::_falling);
- _echo4.fall(this, &RFSRF05::_falling);
- _echo5.fall(this, &RFSRF05::_falling);
-#endif
-
-
- //init callabck function
- callbackfunc = NULL;
- callbackobj = NULL;
- mcallbackfunc = NULL;
-
- // innitialises beacon counter
- _beacon_counter = 0;
-
-#ifdef ROBOT_PRIMARY
- //Interrupts every 50ms for primary robot
- _ticker.attach(this, &RFSRF05::_startRange, 0.05);
-#else
- //attach callback
- _rf.callbackobj = (DummyCT*)this;
- _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
-#endif
-
-}
-
-#ifdef ROBOT_PRIMARY
-void RFSRF05::_startRange() {
-
- //printf("Srange\r\r");
-
- // increments counter
- _beacon_counter = (_beacon_counter + 1) % 3;
-
-
- // set flags
- ValidPulse = false;
- expValidPulse = true;
-
- // writes code to RF port
- _rf.write(codes[_beacon_counter]);
-
- // send a trigger pulse, 10uS long
- _trigger = 1;
- wait_us (10);
- _trigger = 0;
-
-}
-#else
-
-void RFSRF05::startRange(unsigned char rx_code) {
- for (int i = 0; i < 3; i++) {
- if (rx_code == codes[i]) {
-
- // assign beacon_counter
- _beacon_counter = i;
-
- // set flags
- ValidPulse = false;
- expValidPulse = true;
-
- // send a trigger pulse, 10uS long
- _trigger = 1;
- wait_us (10);
- _trigger = 0;
- break;
- }
- }
-}
-#endif
-
-// Clear and start the timer at the begining of the echo pulse
-void RFSRF05::_rising(void) {
-
- _timer.reset();
- _timer.start();
-
- //Set callback execute to ture
- if (expValidPulse) {
- ValidPulse = true;
- expValidPulse = false;
- }
-}
-
-// Stop and read the timer at the end of the pulse
-void RFSRF05::_falling(void) {
- _timer.stop();
-
- if (ValidPulse) {
- //printf("Validpulse trig!\r\n");
- ValidPulse = false;
-
- //Calucate distance
- //true offset is about 100, we put 300 so circles overlap
- _dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
-
- if (callbackfunc)
- (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
-
- if (callbackobj && mcallbackfunc)
- (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
-
- }
-
-}
-
-float RFSRF05::read0() {
- // returns distance
- return (_dist[0]);
-}
-
-float RFSRF05::read1() {
- // returns distance
- return (_dist[1]);
-}
-
-float RFSRF05::read2() {
- // returns distance
- return (_dist[2]);
-}
-
-float RFSRF05::read(unsigned int beaconnum) {
- // returns distance
- return (_dist[beaconnum]);
-}
-
-void RFSRF05::setCode(int code_index, unsigned char code) {
- codes[code_index] = code;
-}
-
-//SRF05::operator float() {
-// return read();
-//}
--- a/Kalman/Sonar/RFSRF05.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,102 +0,0 @@
-
-#ifndef MBED_RFSRF05_H
-#define MBED_RFSRF05_H
-
-
-
-#include "mbed.h"
-#include "RF12B.h"
-#include "globals.h"
-
-
-#define CODE0 0x22
-#define CODE1 0x44
-#define CODE2 0x88
-
-/* SAMPLE IMPLEMENTATION!
-RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
-
-
-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 RFSRF05 {
-public:
-
- RFSRF05(
- PinName trigger,
- PinName echo0,
- PinName echo1,
- PinName echo2,
- PinName echo3,
- PinName echo4,
- PinName echo5,
- PinName SDI,
- PinName SDO,
- PinName SCK,
- PinName NCS,
- PinName NIRQ);
-
- /** A non-blocking function that will return the last measurement
- *
- * @returns floating point representation of distance in mm
- */
- float read0();
- float read1();
- float read2();
- float read(unsigned int beaconnum);
-
-
- /** A assigns a callback function when a new reading is available **/
- void (*callbackfunc)(int beaconnum, float distance);
- DummyCT* callbackobj;
- void (DummyCT::*mcallbackfunc)(int beaconnum, float distance, float variance);
-
- //triggers a read
- #ifndef ROBOT_PRIMARY
- void startRange(unsigned char rx_code);
- #endif
-
- //set codes
- void setCode(int code_index, unsigned char code);
- unsigned char codes[3];
-
- /** A short hand way of using the read function */
- //operator float();
-
-private :
- RF12B _rf;
- DigitalOut _trigger;
- InterruptIn _echo0;
- InterruptIn _echo1;
- InterruptIn _echo2;
- InterruptIn _echo3;
- InterruptIn _echo4;
- InterruptIn _echo5;
- Timer _timer;
- Ticker _ticker;
- #ifdef ROBOT_PRIMARY
- void _startRange(void);
- #endif
- void _rising (void);
- void _falling (void);
- float _dist[3];
- int _beacon_counter;
- bool ValidPulse;
- bool expValidPulse;
-
-};
-
-#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar/RF12B/RF12B.cpp Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,400 @@
+#include "RF12B.h"
+
+#include "RF_defs.h"
+#include <algorithm>
+#include "system.h"
+#include "globals.h"
+
+
+RF12B::RF12B(PinName _SDI,
+ PinName _SDO,
+ PinName _SCK,
+ PinName _NCS,
+ PinName _NIRQ):spi(_SDI, _SDO, _SCK),
+ NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ) {// rfled(LED3) {
+
+ // SPI frequency, word lenght, polarity and phase */
+ spi.format(16,0);
+ spi.frequency(2000000);
+
+ // Set ~CS high
+ NCS = 1;
+
+ // Initialise RF Module
+ init();
+
+ // Setup interrupt to happen on falling edge of NIRQ
+ NIRQ.fall(this, &RF12B::rxISR);
+}
+
+// Returns the packet length if data is available in the receive buffer, 0 otherwise
+//unsigned int RF12B::available() {
+// return fifo.size();
+//}
+
+// Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets
+/*bool RF12B::read(unsigned char* data, unsigned int size) {
+ if (fifo.size() == 0) {
+ return false;
+ } else {
+ unsigned int i = 0;
+ while (fifo.size() > 0 && i < size) {
+ data[i++] = fifo.front();
+ fifo.pop();
+ }
+ return true;
+ }
+}
+*/
+
+// Reads a byte of data from the receive buffer
+/*
+unsigned char RF12B::read() {
+ if (available()) {
+ unsigned char data = fifo.front();
+ fifo.pop();
+ return data;
+ } else {
+ return 0xFF; // Error val although could also be data...
+ }
+}
+*/
+
+// Sends a packet of data to the RF module for transmission TODO: Make asych
+void RF12B::write(unsigned char *data, unsigned char length) {
+ unsigned char crc = 0;
+
+ // Transmitter mode
+ changeMode(TX);
+
+ writeCmd(0x0000);
+ send(0xAA); // PREAMBLE
+ send(0xAA);
+ send(0xAA);
+ send(0x2D); // SYNC
+ send(0xD4);
+ // Packet Length
+ send(length);
+ crc = crc8(crc, length);
+ send(crc);
+ crc = crc8(crc, crc);
+ // Packet Data
+ for (unsigned char i=0; i<length; i++) {
+ send(data[i]);
+ crc = crc8(crc, data[i]);
+ }
+ send(crc);
+ send(0xAA); // DUMMY BYTES
+ send(0xAA);
+ send(0xAA);
+
+ // Back to receiver mode
+ changeMode(RX);
+ status();
+
+
+}
+
+// Transmit a 1-byte data packet
+void RF12B::write(unsigned char data) {
+ write(&data, 1);
+}
+/*
+void RF12B::write(queue<char> &data, int length) {
+ char crc = 0;
+ char length_byte = 0;
+
+ // -1 means try to transmit everything in the queue
+ if (length == -1) {
+ length = data.size();
+ }
+
+ // max length of packet is 255
+ length_byte = min(length, 255);
+
+ // Transmitter mode
+ changeMode(TX);
+
+ writeCmd(0x0000);
+ send(0xAA); // PREAMBLE
+ send(0xAA);
+ send(0xAA);
+ send(0x2D); // SYNC
+ send(0xD4);
+ // Packet Length
+ send(length_byte);
+ crc = crc8(crc, length_byte);
+ send(crc);
+ crc = crc8(crc, crc);
+ // Packet Data
+ for (char i=0; i<length_byte; i++) {
+ send(data.front());
+ crc = crc8(crc, data.front());
+ data.pop();
+ }
+ send(crc);
+ send(0xAA); // DUMMY BYTES
+ send(0xAA);
+ send(0xAA);
+
+ // Back to receiver mode
+ changeMode(RX);
+ status();
+}
+*/
+/**********************************************************************
+ * PRIVATE FUNCTIONS
+ *********************************************************************/
+
+// Initialises the RF12B module
+void RF12B::init() {
+ // writeCmd(0x80E7); //EL,EF,868band,12.0pF
+ changeMode(RX);
+ writeCmd(0xA640); //frequency select
+ writeCmd(0xC647); //4.8kbps
+ writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
+ writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
+ writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
+ writeCmd(0xCED4); //SYNC=2DD4
+ writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
+ writeCmd(0x9850); //!mp,90kHz,MAX OUT
+ writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT�CBW0
+ writeCmd(0xE000); //NOT USED
+ writeCmd(0xC800); //NOT USED
+ writeCmd(0xC040); //1.66MHz,2.2V
+
+ writeCmd(
+ RFM_CONFIG_EL |
+ RFM_CONFIG_EF |
+ RFM_CONFIG_BAND_433 //|
+ //RFM_CONFIG_X_11_0pf // meh, using default
+ );
+
+ // 2. Power Management Command
+ // leave everything switched off for now
+ /*
+ writeCmd(
+ RFM_POWER_MANAGEMENT // switch all off
+ );
+ */
+
+ // 3. Frequency Setting Command
+ writeCmd(
+ RFM_FREQUENCY |
+ RFM_FREQ_433Band(435.7) //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
+ );
+
+
+ // 4. Data Rate Command
+ //writeCmd(RFM_DATA_RATE_9600);
+ writeCmd(RFM_DATA_RATE_57600);
+
+
+ // 5. Receiver Control Command
+ writeCmd(
+ RFM_RX_CONTROL_P20_VDI |
+ RFM_RX_CONTROL_VDI_FAST |
+ //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
+ RFM_RX_CONTROL_BW_134 | // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
+ RFM_RX_CONTROL_GAIN_0 |
+ RFM_RX_CONTROL_RSSI_103 // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
+ );
+
+ // 6. Data Filter Command
+ writeCmd(
+ RFM_DATA_FILTER_AL |
+ RFM_DATA_FILTER_ML |
+ RFM_DATA_FILTER_DIG //|
+ //RFM_DATA_FILTER_DQD(4)
+ );
+
+ // 7. FIFO and Reset Mode Command
+ writeCmd(
+ RFM_FIFO_IT(8) |
+ RFM_FIFO_DR |
+ 0x8 //turn on 16bit sync word
+ );
+
+ // 8. FIFO Syncword
+ // Leave as default: 0xD4
+
+ // 9. Receiver FIFO Read
+ // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
+ // result = RFM_READ_FIFO();
+
+ // 10. AFC Command
+ writeCmd(
+ //RFM_AFC_AUTO_VDI | //Note this might be changed to improve range. Refer to datasheet.
+ RFM_AFC_AUTO_INDEPENDENT |
+ RFM_AFC_RANGE_LIMIT_7_8 |
+ RFM_AFC_EN |
+ RFM_AFC_OE |
+ RFM_AFC_FI
+ );
+
+ // 11. TX Configuration Control Command
+ writeCmd(
+ RFM_TX_CONTROL_MOD_60 |
+ RFM_TX_CONTROL_POW_0
+ );
+
+
+ // 12. PLL Setting Command
+ writeCmd(
+ 0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
+ // I think this will slow down the pll's reaction time. Not sure, check with someone!
+ );
+
+ changeMode(RX);
+ resetRX();
+ status();
+}
+
+/* Write a command to the RF Module */
+unsigned int RF12B::writeCmd(unsigned int cmd) {
+ NCS = 0;
+ unsigned int recv = spi.write(cmd);
+ NCS = 1;
+ return recv;
+}
+
+/* Sends a byte of data across RF */
+void RF12B::send(unsigned char data) {
+ while (NIRQ);
+ writeCmd(0xB800 + data);
+}
+
+/* Change the mode of the RF module to Transmitting or Receiving */
+void RF12B::changeMode(rfmode_t _mode) {
+ mode = _mode;
+ if (_mode == TX) {
+ writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
+ } else { /* mode == RX */
+ writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
+ }
+}
+
+// Interrupt routine for data reception */
+void RF12B::rxISR() {
+
+ unsigned int data = 0;
+ static int i = -2;
+ static unsigned char packet_length = 0;
+ static unsigned char crc = 0;
+// #ifdef ROBOT_SECONDARY
+ static unsigned char temp;
+// #endif
+
+ //Loop while interrupt is asserted
+ while (!NIRQ_in && mode == RX) {
+
+ // Grab the packet's length byte
+ if (i == -2) {
+ data = writeCmd(0x0000);
+ if ( (data&0x8000) ) {
+ data = writeCmd(0xB000);
+ packet_length = (data&0x00FF);
+ crc = crc8(crc, packet_length);
+ i++;
+ }
+ }
+
+ //If we exhaust the interrupt, exit
+ if (NIRQ_in)
+ break;
+
+ // Check that packet length was correct
+ if (i == -1) {
+ data = writeCmd(0x0000);
+ if ( (data&0x8000) ) {
+ data = writeCmd(0xB000);
+ unsigned char crcofsize = (data&0x00FF);
+ if (crcofsize != crc) {
+ //It was wrong, start over
+ i = -2;
+ packet_length = 0;
+ crc = 0;
+ //temp = queue<unsigned char>();
+ resetRX();
+ } else {
+ crc = crc8(crc, crcofsize);
+ i++;
+ }
+ }
+ }
+
+ //If we exhaust the interrupt, exit
+ if (NIRQ_in)
+ break;
+
+ // Grab the packet's data
+ if (i >= 0 && i < packet_length) {
+ data = writeCmd(0x0000);
+ if ( (data&0x8000) ) {
+ data = writeCmd(0xB000);
+ // #ifdef ROBOT_SECONDARY
+ temp = data&0x00FF;
+ // #endif
+ //temp.push(data&0x00FF);
+ crc = crc8(crc, (unsigned char)(data&0x00FF));
+ i++;
+ }
+ }
+
+ //If we exhaust the interrupt, exit
+ if (NIRQ_in)
+ break;
+
+ if (i >= packet_length) {
+ data = writeCmd(0x0000);
+ if ( (data&0x8000) ) {
+ data = writeCmd(0xB000);
+ if ((unsigned char)(data & 0x00FF) == crc) {
+ //If the checksum is correct, add our data to the end of the output buffer
+ //while (!temp.empty()) {
+ //fifo.push(temp);
+ // temp.pop();
+//#ifdef ROBOT_SECONDARY
+ if (callbackfunc)
+ (*callbackfunc)(temp);
+
+ if (callbackobj && mcallbackfunc)
+ (callbackobj->*mcallbackfunc)(temp);
+//#endif
+ // }
+ }
+
+ // Tell RF Module we are finished, and clean up
+ i = -2;
+ packet_length = 0;
+ crc = 0;
+ //temp = queue<unsigned char>();
+ resetRX();
+ }
+ }
+ }
+
+}
+
+unsigned int RF12B::status() {
+ return writeCmd(0x0000);
+}
+
+// Tell the RF Module this packet is received and wait for the next */
+void RF12B::resetRX() {
+ writeCmd(0xCA81);
+ writeCmd(0xCA83);
+};
+
+// Calculate CRC8 */
+unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
+ crc = crc ^ data;
+ for (int i = 0; i < 8; i++) {
+ if (crc & 0x01) {
+ crc = (crc >> 1) ^ 0x8C;
+ } else {
+ crc >>= 1;
+ }
+ }
+ return crc;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar/RF12B/RF12B.h Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,83 @@
+#ifndef _RF12B_H
+#define _RF12B_H
+
+#include "mbed.h"
+//#include <queue>
+
+enum rfmode_t{RX, TX};
+
+class DummyCT;
+
+class RF12B {
+public:
+ /* Constructor */
+ RF12B(PinName SDI,
+ PinName SDO,
+ PinName SCK,
+ PinName NCS,
+ PinName NIRQ);
+
+
+
+ /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
+ bool read(unsigned char* data, unsigned int size);
+
+ /* Reads a byte of data from the receive buffer
+ Returns 0xFF if there is no data */
+ unsigned char read();
+
+ /* Transmits a packet of data */
+ void write(unsigned char* data, unsigned char length);
+ void write(unsigned char data); /* 1-byte packet */
+// void write(std::queue<char> &data, int length = -1); /* sends a whole queue */
+
+ /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+ unsigned int available();
+
+ /** A assigns a callback function when a new reading is available **/
+ void (*callbackfunc)(unsigned char rx_code);
+ DummyCT* callbackobj;
+ void (DummyCT::*mcallbackfunc)(unsigned char rx_code);
+
+protected:
+ /* Receive FIFO buffer */
+// std::queue<unsigned char> fifo;
+// std::queue<unsigned char> temp; //for storing stuff mid-packet
+
+ /* SPI module */
+ SPI spi;
+
+ /* Other digital pins */
+ DigitalOut NCS;
+ InterruptIn NIRQ;
+ DigitalIn NIRQ_in;
+ //DigitalOut rfled;
+
+ rfmode_t mode;
+
+ /* Initialises the RF12B module */
+ void init();
+
+ /* Write a command to the RF Module */
+ unsigned int writeCmd(unsigned int cmd);
+
+ /* Sends a byte of data across RF */
+ void send(unsigned char data);
+
+ /* Switch module between receive and transmit modes */
+ void changeMode(rfmode_t mode);
+
+ /* Interrupt routine for data reception */
+ void rxISR();
+
+ /* Tell the RF Module this packet is received and wait for the next */
+ void resetRX();
+
+ /* Return the RF Module Status word */
+ unsigned int status();
+
+ /* Calculate CRC8 */
+ unsigned char crc8(unsigned char crc, unsigned char data);
+};
+
+#endif /* _RF12B_H */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sonar/RF12B/RF_defs.h Sun Apr 07 16:30:49 2013 +0000 @@ -0,0 +1,478 @@ +/* + * Open HR20 + * + * target: ATmega169 @ 4 MHz in Honnywell Rondostat HR20E + * + * compiler: WinAVR-20071221 + * avr-libc 1.6.0 + * GCC 4.2.2 + * + * copyright: 2008 Dario Carluccio (hr20-at-carluccio-dot-de) + * 2008 Jiri Dobry (jdobry-at-centrum-dot-cz) + * 2008 Mario Fischer (MarioFischer-at-gmx-dot-net) + * 2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net) + * + * license: This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU Library General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http:*www.gnu.org/licenses + */ + +/* + * \file rfm.h + * \brief functions to control the RFM12 Radio Transceiver Module + * \author Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net> + * \date $Date: 2010/04/17 17:57:02 $ + * $Rev: 260 $ + */ + + +//#pragma once // multi-iclude prevention. gcc knows this pragma +#ifndef rfm_H +#define rfm_H + + +#define RFM_SPI_16(OUTVAL) rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value) + +#define RFM_CLK_OUTPUT 0 + +/* +#define RFM_TESTPIN_INIT +#define RFM_TESTPIN_ON +#define RFM_TESTPIN_OFF +#define RFM_TESTPIN_TOG + +#define RFM_CONFIG_DISABLE 0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do +#define RFM_CONFIG_BROADCASTSTATUS 0x01 //<! Flag that enables the HR20's status broadcast every minute + +#define RFM_CONFIG_ENABLEALL 0xff +*/ + + +/////////////////////////////////////////////////////////////////////////////// +// +// RFM status bits +// +/////////////////////////////////////////////////////////////////////////////// + +// Interrupt bits, latched //////////////////////////////////////////////////// + +#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits + // Cleared by any FIFO read method + +#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive + // Cleared by TX write + +#define RFM_STATUS_POR 0x4000 // Power On reset + // Cleared by read status + +#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write + // Cleared by read status + +#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow + // Cleared by read status + +#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow + // Cleared by read status + +#define RFM_STATUS_EXT 0x0800 // Interupt changed to low + // Cleared by read status + +#define RFM_STATUS_LBD 0x0400 // Low battery detect + +// Status bits //////////////////////////////////////////////////////////////// + +#define RFM_STATUS_FFEM 0x0200 // FIFO is empty +#define RFM_STATUS_ATS 0x0100 // TX mode: Strong enough RF signal +#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit +#define RFM_STATUS_DQD 0x0080 // Data Quality detector output +#define RFM_STATUS_CRL 0x0040 // Clock recovery lock +#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle + +/////////////////////////////////////////////////////////////////////////////// +// +// 1. Configuration Setting Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_CONFIG 0x8000 + +#define RFM_CONFIG_EL 0x8080 // Enable TX Register +#define RFM_CONFIG_EF 0x8040 // Enable RX FIFO buffer +#define RFM_CONFIG_BAND_315 0x8000 // Frequency band +#define RFM_CONFIG_BAND_433 0x8010 +#define RFM_CONFIG_BAND_868 0x8020 +#define RFM_CONFIG_BAND_915 0x8030 +#define RFM_CONFIG_X_8_5pf 0x8000 // Crystal Load Capacitor +#define RFM_CONFIG_X_9_0pf 0x8001 +#define RFM_CONFIG_X_9_5pf 0x8002 +#define RFM_CONFIG_X_10_0pf 0x8003 +#define RFM_CONFIG_X_10_5pf 0x8004 +#define RFM_CONFIG_X_11_0pf 0x8005 +#define RFM_CONFIG_X_11_5pf 0x8006 +#define RFM_CONFIG_X_12_0pf 0x8007 +#define RFM_CONFIG_X_12_5pf 0x8008 +#define RFM_CONFIG_X_13_0pf 0x8009 +#define RFM_CONFIG_X_13_5pf 0x800A +#define RFM_CONFIG_X_14_0pf 0x800B +#define RFM_CONFIG_X_14_5pf 0x800C +#define RFM_CONFIG_X_15_0pf 0x800D +#define RFM_CONFIG_X_15_5pf 0x800E +#define RFM_CONFIG_X_16_0pf 0x800F + +/////////////////////////////////////////////////////////////////////////////// +// +// 2. Power Management Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_POWER_MANAGEMENT 0x8200 + +#define RFM_POWER_MANAGEMENT_ER 0x8280 // Enable receiver +#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block +#define RFM_POWER_MANAGEMENT_ET 0x8220 // Enable transmitter +#define RFM_POWER_MANAGEMENT_ES 0x8210 // Enable synthesizer +#define RFM_POWER_MANAGEMENT_EX 0x8208 // Enable crystal oscillator +#define RFM_POWER_MANAGEMENT_EB 0x8204 // Enable low battery detector +#define RFM_POWER_MANAGEMENT_EW 0x8202 // Enable wake-up timer +#define RFM_POWER_MANAGEMENT_DC 0x8201 // Disable clock output of CLK pin + +#ifndef RFM_CLK_OUTPUT + #error RFM_CLK_OUTPUT must be defined to 0 or 1 +#endif +#if RFM_CLK_OUTPUT + #define RFM_TX_ON_PRE() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_TX_ON() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_ET | \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_RX_ON() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_ER | \ + RFM_POWER_MANAGEMENT_EBB | \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_OFF() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_EX ) +#else + #define RFM_TX_ON_PRE() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_DC | \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_TX_ON() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_DC | \ + RFM_POWER_MANAGEMENT_ET | \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_RX_ON() RFM_SPI_16( \ + RFM_POWER_MANAGEMENT_DC | \ + RFM_POWER_MANAGEMENT_ER | \ + RFM_POWER_MANAGEMENT_EBB | \ + RFM_POWER_MANAGEMENT_ES | \ + RFM_POWER_MANAGEMENT_EX ) + #define RFM_OFF() RFM_SPI_16(RFM_POWER_MANAGEMENT_DC) +#endif +/////////////////////////////////////////////////////////////////////////////// +// +// 3. Frequency Setting Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_FREQUENCY 0xA000 + +#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000) +#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000) +#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000) +#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000) + +/////////////////////////////////////////////////////////////////////////////// +// +// 4. Data Rate Command +// +///////////////////////////////////////////////////////////////////////////////// + +#define RFM_BAUD_RATE 9600 + +#define RFM_DATA_RATE 0xC600 + +#define RFM_DATA_RATE_CS 0xC680 +#define RFM_DATA_RATE_4800 0xC647 +#define RFM_DATA_RATE_9600 0xC623 +#define RFM_DATA_RATE_19200 0xC611 +#define RFM_DATA_RATE_38400 0xC608 +#define RFM_DATA_RATE_57600 0xC605 + +#define RFM_SET_DATARATE(baud) ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) ) + +/////////////////////////////////////////////////////////////////////////////// +// +// 5. Receiver Control Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_RX_CONTROL 0x9000 + +#define RFM_RX_CONTROL_P20_INT 0x9000 // Pin20 = ExternalInt +#define RFM_RX_CONTROL_P20_VDI 0x9400 // Pin20 = VDI out + +#define RFM_RX_CONTROL_VDI_FAST 0x9000 // fast VDI Response time +#define RFM_RX_CONTROL_VDI_MED 0x9100 // medium +#define RFM_RX_CONTROL_VDI_SLOW 0x9200 // slow +#define RFM_RX_CONTROL_VDI_ON 0x9300 // Always on + +#define RFM_RX_CONTROL_BW_400 0x9020 // bandwidth 400kHz +#define RFM_RX_CONTROL_BW_340 0x9040 // bandwidth 340kHz +#define RFM_RX_CONTROL_BW_270 0x9060 // bandwidth 270kHz +#define RFM_RX_CONTROL_BW_200 0x9080 // bandwidth 200kHz +#define RFM_RX_CONTROL_BW_134 0x90A0 // bandwidth 134kHz +#define RFM_RX_CONTROL_BW_67 0x90C0 // bandwidth 67kHz + +#define RFM_RX_CONTROL_GAIN_0 0x9000 // LNA gain 0db +#define RFM_RX_CONTROL_GAIN_6 0x9008 // LNA gain -6db +#define RFM_RX_CONTROL_GAIN_14 0x9010 // LNA gain -14db +#define RFM_RX_CONTROL_GAIN_20 0x9018 // LNA gain -20db + +#define RFM_RX_CONTROL_RSSI_103 0x9000 // DRSSI threshold -103dbm +#define RFM_RX_CONTROL_RSSI_97 0x9001 // DRSSI threshold -97dbm +#define RFM_RX_CONTROL_RSSI_91 0x9002 // DRSSI threshold -91dbm +#define RFM_RX_CONTROL_RSSI_85 0x9003 // DRSSI threshold -85dbm +#define RFM_RX_CONTROL_RSSI_79 0x9004 // DRSSI threshold -79dbm +#define RFM_RX_CONTROL_RSSI_73 0x9005 // DRSSI threshold -73dbm +//#define RFM_RX_CONTROL_RSSI_67 0x9006 // DRSSI threshold -67dbm // RF12B reserved +//#define RFM_RX_CONTROL_RSSI_61 0x9007 // DRSSI threshold -61dbm // RF12B reserved + +#define RFM_RX_CONTROL_BW(baud) (((baud)<8000) ? \ + RFM_RX_CONTROL_BW_67 : \ + ( \ + ((baud)<30000) ? \ + RFM_RX_CONTROL_BW_134 : \ + RFM_RX_CONTROL_BW_200 \ + )) + +/////////////////////////////////////////////////////////////////////////////// +// +// 6. Data Filter Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_DATA_FILTER 0xC228 + +#define RFM_DATA_FILTER_AL 0xC2A8 // clock recovery auto-lock +#define RFM_DATA_FILTER_ML 0xC268 // clock recovery fast mode +#define RFM_DATA_FILTER_DIG 0xC228 // data filter type digital +#define RFM_DATA_FILTER_ANALOG 0xC238 // data filter type analog +#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7)) + +/////////////////////////////////////////////////////////////////////////////// +// +// 7. FIFO and Reset Mode Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_FIFO 0xCA00 + +#define RFM_FIFO_AL 0xCA04 // FIFO Start condition sync-word/always +#define RFM_FIFO_FF 0xCA02 // Enable FIFO fill +#define RFM_FIFO_DR 0xCA01 // Disable hi sens reset mode +#define RFM_FIFO_IT(level) (RFM_FIFO | (( (level) & 0xF)<<4)) + +#define RFM_FIFO_OFF() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_DR) +#define RFM_FIFO_ON() RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR) + +///////////////////////////////////////////////////////////////////////////// +// +// 8. Receiver FIFO Read +// +///////////////////////////////////////////////////////////////////////////// + +#define RFM_READ_FIFO() (RFM_SPI_16(0xB000) & 0xFF) + +///////////////////////////////////////////////////////////////////////////// +// +// 9. AFC Command +// +///////////////////////////////////////////////////////////////////////////// + +#define RFM_AFC 0xC400 + +#define RFM_AFC_EN 0xC401 +#define RFM_AFC_OE 0xC402 +#define RFM_AFC_FI 0xC404 +#define RFM_AFC_ST 0xC408 + +// Limits the value of the frequency offset register to the next values: + +#define RFM_AFC_RANGE_LIMIT_NO 0xC400 // 0: No restriction +#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres +#define RFM_AFC_RANGE_LIMIT_7_8 0xC420 // 2: +7 fres to -8 fres +#define RFM_AFC_RANGE_LIMIT_3_4 0xC430 // 3: +3 fres to -4 fres + +// fres=2.5 kHz in 315MHz and 433MHz Bands +// fres=5.0 kHz in 868MHz Band +// fres=7.5 kHz in 915MHz Band + +#define RFM_AFC_AUTO_OFF 0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller) +#define RFM_AFC_AUTO_ONCE 0xC440 // 1: Runs only once after each power-up +#define RFM_AFC_AUTO_VDI 0xC480 // 2: Keep the foffset only during receiving(VDI=high) +#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal + +/////////////////////////////////////////////////////////////////////////////// +// +// 10. TX Configuration Control Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_TX_CONTROL 0x9800 + +#define RFM_TX_CONTROL_POW_0 0x9800 +#define RFM_TX_CONTROL_POW_3 0x9801 +#define RFM_TX_CONTROL_POW_6 0x9802 +#define RFM_TX_CONTROL_POW_9 0x9803 +#define RFM_TX_CONTROL_POW_12 0x9804 +#define RFM_TX_CONTROL_POW_15 0x9805 +#define RFM_TX_CONTROL_POW_18 0x9806 +#define RFM_TX_CONTROL_POW_21 0x9807 +#define RFM_TX_CONTROL_MOD_15 0x9800 +#define RFM_TX_CONTROL_MOD_30 0x9810 +#define RFM_TX_CONTROL_MOD_45 0x9820 +#define RFM_TX_CONTROL_MOD_60 0x9830 +#define RFM_TX_CONTROL_MOD_75 0x9840 +#define RFM_TX_CONTROL_MOD_90 0x9850 +#define RFM_TX_CONTROL_MOD_105 0x9860 +#define RFM_TX_CONTROL_MOD_120 0x9870 +#define RFM_TX_CONTROL_MOD_135 0x9880 +#define RFM_TX_CONTROL_MOD_150 0x9890 +#define RFM_TX_CONTROL_MOD_165 0x98A0 +#define RFM_TX_CONTROL_MOD_180 0x98B0 +#define RFM_TX_CONTROL_MOD_195 0x98C0 +#define RFM_TX_CONTROL_MOD_210 0x98D0 +#define RFM_TX_CONTROL_MOD_225 0x98E0 +#define RFM_TX_CONTROL_MOD_240 0x98F0 +#define RFM_TX_CONTROL_MP 0x9900 + +#define RFM_TX_CONTROL_MOD(baud) (((baud)<8000) ? \ + RFM_TX_CONTROL_MOD_45 : \ + ( \ + ((baud)<20000) ? \ + RFM_TX_CONTROL_MOD_60 : \ + ( \ + ((baud)<30000) ? \ + RFM_TX_CONTROL_MOD_75 : \ + ( \ + ((baud)<40000) ? \ + RFM_TX_CONTROL_MOD_90 : \ + RFM_TX_CONTROL_MOD_120 \ + ) \ + ) \ + )) + +///////////////////////////////////////////////////////////////////////////// +// +// 11. Transmitter Register Write Command +// +///////////////////////////////////////////////////////////////////////////// + +//#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | ((byte) & 0xFF)) +#define RFM_WRITE(byte) RFM_SPI_16(0xB800 | (byte) ) + +/////////////////////////////////////////////////////////////////////////////// +// +// 12. Wake-up Timer Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_WAKEUP_TIMER 0xE000 +#define RFM_WAKEUP_SET(time) RFM_SPI_16(RFM_WAKEUP_TIMER | (time)) + +#define RFM_WAKEUP_480s (RFM_WAKEUP_TIMER |(11 << 8)| 234) +#define RFM_WAKEUP_240s (RFM_WAKEUP_TIMER |(10 << 8)| 234) +#define RFM_WAKEUP_120s (RFM_WAKEUP_TIMER |(9 << 8)| 234) +#define RFM_WAKEUP_119s (RFM_WAKEUP_TIMER |(9 << 8)| 232) + +#define RFM_WAKEUP_60s (RFM_WAKEUP_TIMER |(8 << 8) | 235) +#define RFM_WAKEUP_59s (RFM_WAKEUP_TIMER |(8 << 8) | 230) + +#define RFM_WAKEUP_30s (RFM_WAKEUP_TIMER |(7 << 8) | 235) +#define RFM_WAKEUP_29s (RFM_WAKEUP_TIMER |(7 << 8) | 227) + +#define RFM_WAKEUP_8s (RFM_WAKEUP_TIMER |(5 << 8) | 250) +#define RFM_WAKEUP_7s (RFM_WAKEUP_TIMER |(5 << 8) | 219) +#define RFM_WAKEUP_6s (RFM_WAKEUP_TIMER |(6 << 8) | 94) +#define RFM_WAKEUP_5s (RFM_WAKEUP_TIMER |(5 << 8) | 156) +#define RFM_WAKEUP_4s (RFM_WAKEUP_TIMER |(5 << 8) | 125) +#define RFM_WAKEUP_1s (RFM_WAKEUP_TIMER |(2 << 8) | 250) +#define RFM_WAKEUP_900ms (RFM_WAKEUP_TIMER |(2 << 8) | 225) +#define RFM_WAKEUP_800ms (RFM_WAKEUP_TIMER |(2 << 8) | 200) +#define RFM_WAKEUP_700ms (RFM_WAKEUP_TIMER |(2 << 8) | 175) +#define RFM_WAKEUP_600ms (RFM_WAKEUP_TIMER |(2 << 8) | 150) +#define RFM_WAKEUP_500ms (RFM_WAKEUP_TIMER |(2 << 8) | 125) +#define RFM_WAKEUP_400ms (RFM_WAKEUP_TIMER |(2 << 8) | 100) +#define RFM_WAKEUP_300ms (RFM_WAKEUP_TIMER |(2 << 8) | 75) +#define RFM_WAKEUP_200ms (RFM_WAKEUP_TIMER |(2 << 8) | 50) +#define RFM_WAKEUP_100ms (RFM_WAKEUP_TIMER |(2 << 8) | 25) + +/////////////////////////////////////////////////////////////////////////////// +// +// 13. Low Duty-Cycle Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_LOW_DUTY_CYCLE 0xC800 + +/////////////////////////////////////////////////////////////////////////////// +// +// 14. Low Battery Detector Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_LOW_BATT_DETECT 0xC000 +#define RFM_LOW_BATT_DETECT_D_1MHZ 0xC000 +#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020 +#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040 +#define RFM_LOW_BATT_DETECT_D_2MHZ 0xC060 +#define RFM_LOW_BATT_DETECT_D_2_5MHZ 0xC080 +#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0 +#define RFM_LOW_BATT_DETECT_D_5MHZ 0xC0C0 +#define RFM_LOW_BATT_DETECT_D_10MHZ 0xC0E0 + +/////////////////////////////////////////////////////////////////////////////// +// +// 15. Status Read Command +// +/////////////////////////////////////////////////////////////////////////////// + +#define RFM_READ_STATUS() RFM_SPI_16(0x0000) +#define RFM_READ_STATUS_FFIT() SPI_1 (0x00) +#define RFM_READ_STATUS_RGIT RFM_READ_STATUS_FFIT + +/////////////////////////////////////////////////////////////////////////////// + +// RFM air protocol flags: + +#define RFMPROTO_FLAGS_BITASK_PACKETTYPE 0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype +#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST 0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1) +#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND 0b01000000 //!< command packettype (message to hr20, protocol; step 2) +#define RFMPROTO_FLAGS_PACKETTYPE_REPLY 0b10000000 //!< reply packettype (message from hr20, protocol; step 3) +#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL 0b11000000 //!< currently unused packettype + +#define RFMPROTO_FLAGS_BITASK_DEVICETYPE 0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist +#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 0b00010100 //!< topen HR20 device type. 10100 is for decimal 20 + +#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) +#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) +#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_REPLY == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) +#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS) ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) ) +#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS) ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20 == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) ) + +/////////////////////////////////////////////////////////////////////////////// + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar/RFSRF05.cpp Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,183 @@
+
+#include "RFSRF05.h"
+#include "mbed.h"
+#include "globals.h"
+#include "system.h"
+
+
+RFSRF05::RFSRF05(PinName trigger,
+ PinName echo0,
+ PinName echo1,
+ PinName echo2,
+ PinName echo3,
+ PinName echo4,
+ PinName echo5,
+ PinName SDI,
+ PinName SDO,
+ PinName SCK,
+ PinName NCS,
+ PinName NIRQ)
+ : _rf(SDI,SDO,SCK,NCS,NIRQ),
+ _trigger(trigger),
+ _echo0(echo0),
+ _echo1(echo1),
+ _echo2(echo2),
+ _echo3(echo3),
+ _echo4(echo4),
+ _echo5(echo5) {
+
+ // initialises codes
+ codes[0] = CODE0;
+ codes[1] = CODE1;
+ codes[2] = CODE2;
+
+ //set callback execute to true
+ ValidPulse = false;
+
+ // Attach interrupts
+#ifdef SONAR_ECHO_INV
+ // inverted sonar inputs
+ _echo5.fall(this, &RFSRF05::_rising);
+ _echo0.rise(this, &RFSRF05::_falling);
+ _echo1.rise(this, &RFSRF05::_falling);
+ _echo2.rise(this, &RFSRF05::_falling);
+ _echo3.rise(this, &RFSRF05::_falling);
+ _echo4.rise(this, &RFSRF05::_falling);
+ _echo5.rise(this, &RFSRF05::_falling);
+#else
+ _echo5.rise(this, &RFSRF05::_rising);
+ _echo0.fall(this, &RFSRF05::_falling);
+ _echo1.fall(this, &RFSRF05::_falling);
+ _echo2.fall(this, &RFSRF05::_falling);
+ _echo3.fall(this, &RFSRF05::_falling);
+ _echo4.fall(this, &RFSRF05::_falling);
+ _echo5.fall(this, &RFSRF05::_falling);
+#endif
+
+
+ //init callabck function
+ callbackfunc = NULL;
+ callbackobj = NULL;
+ mcallbackfunc = NULL;
+
+ // innitialises beacon counter
+ _beacon_counter = 0;
+
+#ifdef ROBOT_PRIMARY
+ //Interrupts every 50ms for primary robot
+ _ticker.attach(this, &RFSRF05::_startRange, 0.05);
+#else
+ //attach callback
+ _rf.callbackobj = (DummyCT*)this;
+ _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
+#endif
+
+}
+
+#ifdef ROBOT_PRIMARY
+void RFSRF05::_startRange() {
+
+ //printf("Srange\r\r");
+
+ // increments counter
+ _beacon_counter = (_beacon_counter + 1) % 3;
+
+
+ // set flags
+ ValidPulse = false;
+ expValidPulse = true;
+
+ // writes code to RF port
+ _rf.write(codes[_beacon_counter]);
+
+ // send a trigger pulse, 10uS long
+ _trigger = 1;
+ wait_us (10);
+ _trigger = 0;
+
+}
+#else
+
+void RFSRF05::startRange(unsigned char rx_code) {
+ for (int i = 0; i < 3; i++) {
+ if (rx_code == codes[i]) {
+
+ // assign beacon_counter
+ _beacon_counter = i;
+
+ // set flags
+ ValidPulse = false;
+ expValidPulse = true;
+
+ // send a trigger pulse, 10uS long
+ _trigger = 1;
+ wait_us (10);
+ _trigger = 0;
+ break;
+ }
+ }
+}
+#endif
+
+// Clear and start the timer at the begining of the echo pulse
+void RFSRF05::_rising(void) {
+
+ _timer.reset();
+ _timer.start();
+
+ //Set callback execute to ture
+ if (expValidPulse) {
+ ValidPulse = true;
+ expValidPulse = false;
+ }
+}
+
+// Stop and read the timer at the end of the pulse
+void RFSRF05::_falling(void) {
+ _timer.stop();
+
+ if (ValidPulse) {
+ //printf("Validpulse trig!\r\n");
+ ValidPulse = false;
+
+ //Calucate distance
+ //true offset is about 100, we put 300 so circles overlap
+ _dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
+
+ if (callbackfunc)
+ (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
+
+ if (callbackobj && mcallbackfunc)
+ (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
+
+ }
+
+}
+
+float RFSRF05::read0() {
+ // returns distance
+ return (_dist[0]);
+}
+
+float RFSRF05::read1() {
+ // returns distance
+ return (_dist[1]);
+}
+
+float RFSRF05::read2() {
+ // returns distance
+ return (_dist[2]);
+}
+
+float RFSRF05::read(unsigned int beaconnum) {
+ // returns distance
+ return (_dist[beaconnum]);
+}
+
+void RFSRF05::setCode(int code_index, unsigned char code) {
+ codes[code_index] = code;
+}
+
+//SRF05::operator float() {
+// return read();
+//}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar/RFSRF05.h Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,102 @@
+
+#ifndef MBED_RFSRF05_H
+#define MBED_RFSRF05_H
+
+
+
+#include "mbed.h"
+#include "RF12B.h"
+#include "globals.h"
+
+
+#define CODE0 0x22
+#define CODE1 0x44
+#define CODE2 0x88
+
+/* SAMPLE IMPLEMENTATION!
+RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
+
+
+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 RFSRF05 {
+public:
+
+ RFSRF05(
+ PinName trigger,
+ PinName echo0,
+ PinName echo1,
+ PinName echo2,
+ PinName echo3,
+ PinName echo4,
+ PinName echo5,
+ PinName SDI,
+ PinName SDO,
+ PinName SCK,
+ PinName NCS,
+ PinName NIRQ);
+
+ /** A non-blocking function that will return the last measurement
+ *
+ * @returns floating point representation of distance in mm
+ */
+ float read0();
+ float read1();
+ float read2();
+ float read(unsigned int beaconnum);
+
+
+ /** A assigns a callback function when a new reading is available **/
+ void (*callbackfunc)(int beaconnum, float distance);
+ DummyCT* callbackobj;
+ void (DummyCT::*mcallbackfunc)(int beaconnum, float distance, float variance);
+
+ //triggers a read
+ #ifndef ROBOT_PRIMARY
+ void startRange(unsigned char rx_code);
+ #endif
+
+ //set codes
+ void setCode(int code_index, unsigned char code);
+ unsigned char codes[3];
+
+ /** A short hand way of using the read function */
+ //operator float();
+
+private :
+ RF12B _rf;
+ DigitalOut _trigger;
+ InterruptIn _echo0;
+ InterruptIn _echo1;
+ InterruptIn _echo2;
+ InterruptIn _echo3;
+ InterruptIn _echo4;
+ InterruptIn _echo5;
+ Timer _timer;
+ Ticker _ticker;
+ #ifdef ROBOT_PRIMARY
+ void _startRange(void);
+ #endif
+ void _rising (void);
+ void _falling (void);
+ float _dist[3];
+ int _beacon_counter;
+ bool ValidPulse;
+ bool expValidPulse;
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs/geometryfuncs.h Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,18 @@
+#ifndef GEOMETRYFUNCS_H
+#define GEOMETRYFUNCS_H
+
+// 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
--- a/globals.h Wed Nov 14 17:15:53 2012 +0000 +++ b/globals.h Sun Apr 07 16:30:49 2013 +0000 @@ -1,12 +1,30 @@ #ifndef GLOBALS_H #define GLOBALS_H +#include "mbed.h" +#define PI 3.14159265 #define ROBOT_PRIMARY +// Stepper motor 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) -#include "mbed.h" -#define PI 3.14159265 +// 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 + + + + //enables ui //#define UION @@ -15,10 +33,6 @@ #ifdef ROBOT_PRIMARY // Primary defs go here -#else -// Secondary defs go here -#endif - //Robot hardware parameters const int robotCircumference = 816; //mm (DUMMY!) @@ -27,6 +41,19 @@ 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;
--- a/main.cpp Wed Nov 14 17:15:53 2012 +0000
+++ b/main.cpp Sun Apr 07 16:30:49 2013 +0000
@@ -1,10 +1,152 @@
#include "mbed.h"
+#include "rtos.h"
+#include "math.h"
#include "globals.h"
+#include "RFSRF05.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
+
+RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
+
+
+
+// RTOS stuff
+Semaphore serial_sema(1);
+Semaphore IR_timeout_sema(1);
+
+// IR timeout timer
+Timeout IR_Timeout;
+
+// 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;
+
+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();
+}
+
+
+
+void IR_Callback(float pulsewidth)
+{
+ static int step_count_local;
-bool Colour = 1; // 1 for red, 0 for blue
-pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start
+ 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)
+{
+ //Here is where you deal with your brand new reading ;D
+ OLED2 = !OLED2;
+ sonar_dist[num] = dist;
+ //serial_sema.release();
+
+}
+
+
+
-//note that nothing is running as of now.
-int main() {
- while(1);
+void serial_thread(void const *argument)
+{
+ 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]);
+ }
}
+
+int main()
+{
+ pc.baud(19200);
+ 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
+
+
+ //some timer counters
+ //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
+
+ while (true) {
+ Thread::wait(osWaitForever);
+ }
+}
+
--- a/mbed-rtos.lib Wed Nov 14 17:15:53 2012 +0000 +++ b/mbed-rtos.lib Sun Apr 07 16:30:49 2013 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed-rtos/#9654a71f5a90 +http://mbed.org/users/mbed_official/code/mbed-rtos/#53e6cccd8782
--- a/mbed.bld Wed Nov 14 17:15:53 2012 +0000 +++ b/mbed.bld Sun Apr 07 16:30:49 2013 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/e2ed12d17f06 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file
--- a/system/geometryfuncs.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,28 +0,0 @@
-#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
--- a/system/system.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,29 +0,0 @@
-#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
--- a/system/system.h Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-
-#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
--- a/ui/ui.cpp Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,103 +0,0 @@
-
-#include "ui.h"
-#include <iostream>
-#include "system.h"
-
-UI::UI() :
- tUI(printtw,this,osPriorityNormal,2048) {
- newdataflags = 0;
- for (int i = 0; i < NUMIDS; i++) {
- idlist[i] = 0;
- buffarr[i] = 0;
- }
-
-}
-
-bool UI::regid(char id, unsigned int length) {
-
- //check if the id is already taken
- if (id < NUMIDS && !idlist[id]) {
- idlist[id] = length;
- buffarr[id] = new float[length];
- return true;
- } else
- return false;
-}
-
-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{
- return false;
- }
-}
-
-bool UI::updateval(char id, float value) {
-
- //check if the id is registered, and the old value has been written
- if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) {
- buffarr[id][0] = value;
- newdataflags |= (1<<id);
- return true;
- } else
- return false;
-
-}
-
-bool UI::unregid(char id) {
- if (id < NUMIDS) {
- idlist[id] = 0;
- if (buffarr[id])
- delete buffarr[id];
- return true;
- } else
- return false;
-}
-
-void UI::printloop() {
-
-#ifdef UION
- Thread::wait(3500);
-#else
- Thread::wait(osWaitForever);
-#endif
-
- char* sync = "ABCD";
- std::cout.write(sync, 4);
- //std::cout.flush();
- std::cout << std::endl;
- //printf("\r\n");
-
- 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 (char id = 0; id < NUMIDS; id++) {
- if (newdataflags & (1<<id)) {
- std::cout.put(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 Wed Nov 14 17:15:53 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-
-#ifndef UI_H
-#define UI_H
-
-#include "rtos.h"
-
-#define NUMIDS 32
-
-class UI {
-public:
- Thread tUI;
-
- UI();
-
- bool regid(char id, unsigned int length);
- bool updateval(char id, float* buffer, unsigned int length);
- bool updateval(char id, float value);
- bool unregid(char id);
-
-private:
- Mutex printlock;
- char idlist[NUMIDS];
- float* buffarr[NUMIDS];
- volatile int newdataflags; //Only works for NUMID = 32
-
- void printloop();
- static void printtw(void const *arg){ ((UI*)arg)->printloop(); }
-};
-
-#endif //UI_H