Shuto Naruse
Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Revision 0:f3bf6c7e2283, committed 2012-04-20
- Comitter:
- narshu
- Date:
- Fri Apr 20 20:39:35 2012 +0000
- Child:
- 1:bbabbd997d21
- Commit message:
- inverted sonar echo input at pin14 and pin15
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,243 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "rtos.h"
+#include "RFSRF05.h"
+//#include "MatrixMath.h"
+//#include "Matrix.h"
+#include "math.h"
+#include "globals.h"
+#include "motors.h"
+#include "system.h"
+
+#include <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+Kalman::Kalman(Motors &motorsin) :
+ sonararray(p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11),
+ motors(motorsin),
+ predictthread(predictloopwrapper, this, osPriorityNormal, 512),
+ predictticker( SIGTICKARGS(predictthread, 0x1) ),
+// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
+// sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
+ updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
+
+ //Initilising matrices
+
+ // X = x, y, theta;
+ X = 0.5, 0, 0;
+
+ P = 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 0.04;
+
+ Q = 0.002, 0, 0, //temporary matrix, Use dt!
+ 0, 0.002, 0,
+ 0, 0, 0.002;
+
+ //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);
+
+
+}
+
+
+void Kalman::predictloop() {
+ Matrix<float, 3, 3> F;
+ int lastleft = motors.getEncoder1();
+ int lastright = motors.getEncoder2();
+ int leftenc;
+ int rightenc;
+ float dleft,dright;
+ float dxp, dyp;
+ float thetap,d,r;
+
+ while (1) {
+// Thread::signal_wait(0x1);
+ led1 = !led1;
+
+ leftenc = motors.getEncoder1();
+ rightenc = motors.getEncoder2();
+
+ dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
+ dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
+
+ lastleft = leftenc;
+ lastright = rightenc;
+
+
+ //The below calculation are in body frame (where +x is forward)
+ 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();
+
+ //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
+ F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
+ 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
+ 0, 0, 1;
+
+
+ P = F * P * trans(F) + Q;
+
+ statelock.unlock();
+ Thread::wait(20);
+
+ //cout << "predict" << X << endl;
+ //cout << P << endl;
+ }
+}
+
+//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)
+ led4 = 1;
+ // printf("putting in MQ error code %#x\r\n", putret);
+ } else {
+ led4 = 1;
+ //printf("MQalloc returned NULL ptr\r\n");
+ }
+
+}
+
+void Kalman::updateloop() {
+ 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) {
+ led2 = !led2;
+
+ 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) {
+
+ float dist = value / 1000.0f; //converting to m from mm
+ int sonarid = type;
+ aborton2stddev = true;
+
+ statelock.lock();
+ SonarMeasures[sonarid] = dist; //update the current sonar readings
+
+ rbx = X(0) - beaconpos[sonarid].x/1000.0f;
+ rby = X(1) - beaconpos[sonarid].y/1000.0f;
+
+ expecdist = sqrt(rbx*rbx + rby*rby);
+ Y = dist - expecdist;
+
+ dhdx = rbx / expecdist;
+ dhdy = rby / expecdist;
+
+ H = dhdx, dhdy, 0;
+
+ } else if (type <= IR3) {
+
+ int IRidx = type-3;
+
+ statelock.lock();
+ IRMeasures[IRidx] = value;
+
+ rbx = X(0) - beaconpos[IRidx].x/1000.0f;
+ rby = X(1) - beaconpos[IRidx].y/1000.0f;
+
+ float expecang = atan2(-rbx, -rby) - X(2);
+ Y = rectifyAng(value - expecang);
+
+ 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 {
+ led4 = 1;
+ //printf("ERROR: in updateloop, code %#x", evt);
+ }
+
+ }
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,63 @@
+#include "rtos.h"
+//#include "Matrix.h"
+#include "motors.h"
+#include "RFSRF05.h"
+
+#include <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+
+
+class Kalman {
+public:
+ enum measurement_t {SONAR1, SONAR2, SONAR3, IR1, IR2, IR3};
+ static const measurement_t maxmeasure = IR3;
+
+ Kalman(Motors &motorsin);
+
+ 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];
+
+private:
+
+ Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
+
+ RFSRF05 sonararray;
+ Motors& motors;
+
+ 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(); }
+
+
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Sonar/.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/narshu/libraries/Sonar/m7pll8 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RF12B.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,393 @@
+#include "RF12B.h"
+
+#include "RF_defs.h"
+#include <algorithm>
+
+//#include "globals.h"
+
+//DigitalOut DBG2(LED2);
+//DigitalOut DBG3(LED3);
+//DigitalOut DBG4(LED4);
+
+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);
+
+
+ // 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() {
+
+ //static int cnt = 0;
+ //printf("%d hits\r\n", cnt);
+ //cnt++;
+
+ //DBG2 = !(cnt%3);
+ //DBG3 = !((cnt+1)%3);
+ //DBG4 = !((cnt+2)%3);
+
+ unsigned int data = 0;
+ static int i = -2;
+ static unsigned char packet_length = 0;
+ static unsigned char crc = 0;
+
+ //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);
+ //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.front());
+ // temp.pop();
+ //}
+ }
+
+ // 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/Kalman/Sonar/RF12B/RF12B.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,75 @@
+#ifndef _RF12B_H
+#define _RF12B_H
+
+#include "mbed.h"
+#include <queue>
+
+enum rfmode_t{RX, TX};
+
+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();
+
+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/Kalman/Sonar/RF12B/RFSerial.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,25 @@
+
+/* Constructor */
+#include "RFSerial.h"
+
+RFSerial::RFSerial(PinName _SDI,
+ PinName _SDO,
+ PinName _SCK,
+ PinName _NCS,
+ PinName _NIRQ)
+:RF12B(_SDI, _SDO, _SCK, _NCS, _NIRQ) {
+
+}
+
+// Stream implementation functions
+int RFSerial::_putc(int value) {
+ RF12B::write((unsigned char) value);
+ return value;
+}
+int RFSerial::_getc() {
+ if(available()) {
+ return RF12B::read();
+ } else {
+ return EOF;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RF12B/RFSerial.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+#include "RF12B.h"
+
+class RFSerial : public Stream, public RF12B {
+public:
+ /* Constructor */
+ RFSerial(PinName _SDI,
+ PinName _SDO,
+ PinName _SCK,
+ PinName _NCS,
+ PinName _NIRQ);
+
+protected:
+ // Stream implementation functions
+ virtual int _putc(int value);
+ virtual int _getc();
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Sonar/RF12B/RF_defs.h Fri Apr 20 20:39:35 2012 +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/Kalman/Sonar/RFSRF05.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,137 @@
+
+#include "RFSRF05.h"
+#include "mbed.h"
+#include "globals.h"
+DigitalOut GLED3(LED3);
+DigitalOut GLED4(LED4);
+
+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
+ _code[0] = CODE0;
+ _code[1] = CODE1;
+ _code[2] = CODE2;
+
+ //set callback execute to true
+ ValidPulse = false;
+
+ // Attach interrupts
+ _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);
+
+
+ //init callabck function
+ callbackfunc = NULL;
+ callbackobj = NULL;
+ mcallbackfunc = NULL;
+
+ // innitialises beacon counter
+ _beacon_counter = 0;
+
+ //Interrupts every 50ms
+ _ticker.attach(this, &RFSRF05::_startRange, 0.05);
+}
+
+
+void RFSRF05::_startRange() {
+
+ //printf("Srange\r\r");
+
+ // increments counter
+ _beacon_counter = (_beacon_counter + 1) % 3;
+
+ // writes code to RF port
+ _rf.write(_code[_beacon_counter]);
+
+ // send a trigger pulse, 10uS long
+ ValidPulse = false;
+ expValidPulse = true;
+
+ _trigger = 1;
+ wait_us (10);
+ _trigger = 0;
+ wait_us(50);
+}
+
+
+// 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
+ _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]);
+}
+
+//SRF05::operator float() {
+// return read();
+//}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Sonar/RFSRF05.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,91 @@
+
+#ifndef MBED_RFSRF05_H
+#define MBED_RFSRF05_H
+
+#include "mbed.h"
+#include "RF12B.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
+
+
+ /** 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;
+ void _startRange();
+ void _rising (void);
+ void _falling (void);
+ float _dist[3];
+ char _code[3];
+ int _beacon_counter;
+ bool ValidPulse;
+ bool expValidPulse;
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/tvmet.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/madcowswe/libraries/tvmet/m7lmpz \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/libraries/PID/le4dor \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/libraries/QEI/le4bkf \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TSH.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,62 @@
+#ifndef TSH_H
+#define TSH_H
+
+#include "rtos.h"
+
+//Thread Safe Hardware
+
+class TSI2C : public I2C {
+public:
+
+ TSI2C( PinName sda,
+ PinName scl,
+ const char* name=NULL )
+ : I2C(sda, scl, name) { }
+
+
+ int read( int address,
+ char* data,
+ int length,
+ bool repeated = false ) {
+
+ rlock.lock();
+ int retval = I2C::read(address, data, length, repeated);
+ rlock.unlock();
+
+ return retval;
+ }
+
+ int read(int ack) {
+ rlock.lock();
+ int retval = I2C::read(ack);
+ rlock.unlock();
+
+ return retval;
+ }
+
+ int write( int address,
+ const char* data,
+ int length,
+ bool repeated = false ) {
+
+ wlock.lock();
+ int retval = I2C::write(address, data, length, repeated);
+ wlock.unlock();
+
+ return retval;
+ }
+
+ int write(int data) {
+ wlock.lock();
+ int retval = I2C::write(data);
+ wlock.unlock();
+
+ return retval;
+ }
+
+private:
+ Mutex rlock;
+ Mutex wlock;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,57 @@
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include "mbed.h"
+
+#define PI 3.14159265
+
+//Robot constants
+//const int encoderRevCount = 1856;
+//const int wheelmm = 314;
+//const int robotCircumference = 1256;
+
+//Robot constants in mm
+const int robot_width = 260;
+const int encoderRevCount = 360;
+const int wheelmm = 226;
+const int robotCircumference = 816;
+
+//Arena constants
+struct pos {
+ int x;
+ int y;
+};
+const pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
+
+//sonar constants
+static const float sonarvariance = 0.004;
+
+//High speed serial port
+extern Serial pc;
+
+//I2C mutex
+//extern Mutex i2c_rlock;
+//extern Mutex i2c_wlock;
+
+
+// IR angle calc
+#define RELI_BOUND_LOW 4
+#define RELI_BOUND_HIGH 25
+
+// Localization estimate torrelences
+#define POSITION_TOR 80
+#define ANGLE_TOR 0.15
+
+// motion control
+#define ROTA_SPEED 40
+#define MOVE_SPEED 50
+#define MAX_STEP_RATIO 0.10 //maximum change in the speed
+//#define TRACK_RATE 10 // +- rate for each wheel when tracking
+
+// Task suspend periods
+#define IR_TURRET_PERIOD 200
+#define MOTION_UPDATE_PERIOD 20
+
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,374 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "TSH.h"
+#include "Kalman.h"
+#include "globals.h"
+#include "motors.h"
+#include "math.h"
+#include "system.h"
+
+//#include <iostream>
+
+//Interface declaration
+//I2C i2c(p28, p27); // sda, scl
+TSI2C i2c(p28, p27);
+Serial pc(USBTX, USBRX); // tx, rx
+Serial IRturret(p13, p14);
+
+DigitalOut OLED1(LED1);
+DigitalOut OLED2(LED2);
+DigitalOut OLED3(LED3);
+DigitalOut OLED4(LED4);
+
+int16_t face;
+
+
+Motors motors;
+Kalman kalman(motors);
+
+float targetX = 1000, targetY = 1000, targetTheta = 0;
+
+// bytes packing for IR turret serial comm
+union IRValue_t {
+ float IR_floats[6];
+ int IR_ints[6];
+ unsigned char IR_chars[24];
+} IRValues;
+
+//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
+//NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
+
+void vMotorThread(void const *argument);
+void vPrintState(void const *argument);
+void ai_thread (void const *argument);
+void motion_thread(void const *argument);
+void vIRValueISR (void);
+void motorSpeedThread(void const *argument);
+
+float getAngle (float x, float y);
+void getIRValue(void const *argument);
+
+
+//Thread thr_AI(ai_thread);
+//Thread thr_motion(motion_thread);
+//Thread thr_getIRValue(getIRValue);
+
+Mutex targetlock;
+bool flag_terminate = false;
+
+
+float temp = 0;
+
+//Main loop
+int main() {
+motors.stop();
+ pc.baud(115200);
+ IRturret.baud(9600);
+ IRturret.format(8,Serial::Odd,1);
+ //Thread thread(motorSpeedThread, NULL, osPriorityAboveNormal); //PID controller task. run at 50ms
+ //IRturret.attach(&vIRValueISR);
+ //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
+ Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
+
+ pc.printf("We got to main! ;D\r\n");
+
+ //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
+ while (1) {
+ // do nothing
+ Thread::wait(osWaitForever);
+ }
+}
+
+void motorSpeedThread(void const *argument) {
+ while (1) {
+ motors.speedRegulatorTask();
+ Thread::wait(10);
+ }
+}
+
+
+void vMotorThread(void const *argument) {
+ motors.resetEncoders();
+ while (1) {
+ motors.setSpeed(20,-20);
+ Thread::wait(2000);
+ motors.stop();
+ Thread::wait(5000);
+ motors.setSpeed(-20,20);
+ Thread::wait(2000);
+ motors.stop();
+ Thread::wait(5000);
+ }
+}
+
+
+
+void vPrintState(void const *argument) {
+ float state[3];
+ float sonardist[3];
+ //float x,y;
+ //float d = beaconpos[2].y - beaconpos[1].y;
+ //float i = beaconpos[0].y;
+ //float j = beaconpos[0].x;
+
+ while (1) {
+ kalman.statelock.lock();
+ state[0] = kalman.X(0);
+ state[1] = kalman.X(1);
+ state[2] = kalman.X(2);
+ sonardist[0] = kalman.SonarMeasures[0]*1000.0f;
+ sonardist[1] = kalman.SonarMeasures[1]*1000.0f;
+ sonardist[2] = kalman.SonarMeasures[2]*1000.0f;
+ kalman.statelock.unlock();
+
+ // trilateration algorithm
+ //y = ((sonardist[1]*sonardist[1]) - (sonardist[2]*sonardist[2]) + d*d) / (2*d);
+ //x = ((sonardist[1]*sonardist[1]) - (sonardist[0]*sonardist[0]) + i*i + j*j) / (2*j) - (i/j)*y;
+
+
+ //kalman.statelock.lock();
+ pc.printf("\r\n");
+ //printf("Position X: %0.1f Y: %0.1f \r\n", x,y);
+ pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
+ //targetlock.lock();
+ //pc.printf("target : %0.4f %0.4f %0.4f \r\n", targetX/1000, targetY/1000,targetTheta);
+ //targetlock.unlock();
+ printf("Sonar Dist: %0.4f, %0.4f, %0.4f \r\n", sonardist[0],sonardist[1],sonardist[2]);
+
+ //printf("Distance is %0.1f, %0.1f \r\n", motors.encoderToDistance(motors.getEncoder1()), motors.encoderToDistance(motors.getEncoder2()));
+
+ //pc.printf("IR ang : %f \n",temp * 180/3.14);
+ //kalman.statelock.unlock();
+ Thread::wait(200);
+ }
+}
+
+
+// AI thread ------------------------------------
+void ai_thread (void const *argument) {
+ // goes to the mid
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 1500;
+ targetY = 1000;
+ targetTheta = PI/2;
+ targetlock.unlock();
+
+ // left roll
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 500;
+ targetY = 1700;
+ targetTheta = PI/2;
+ targetlock.unlock();
+
+ // mid
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 1500;
+ targetY = 1000;
+ targetTheta = PI/2;
+ targetlock.unlock();
+
+ // map
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 1500;
+ targetY = 1700;
+ targetTheta = PI/2;
+ targetlock.unlock();
+
+ // mid
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 1500;
+ targetY = 1000;
+ targetTheta = -PI;
+ targetlock.unlock();
+
+ // home
+ Thread::signal_wait(0x01);
+ targetlock.lock();
+ targetX = 500;
+ targetY = 500;
+ targetTheta = 0;
+ targetlock.unlock();
+
+ Thread::signal_wait(0x01);
+ flag_terminate = true;
+ //OLED3 = true;
+
+ while (true) {
+ Thread::wait(osWaitForever);
+ }
+}
+
+// motion control thread ------------------------
+void motion_thread(void const *argument) {
+ motors.resetEncoders();
+ motors.setSpeed(20,20);
+ Thread::wait(2000);
+ motors.stop();
+ //thr_AI.signal_set(0x01);
+
+
+
+ float currX, currY,currTheta;
+ float speedL,speedR;
+ float diffDir,diffSpeed;
+ float lastdiffSpeed = 0;
+
+ while (1) {
+ if (flag_terminate) {
+ terminate();
+ }
+
+ // get kalman localization estimate ------------------------
+ kalman.statelock.lock();
+ currX = kalman.X(0)*1000.0f;
+ currY = kalman.X(1)*1000.0f;
+ currTheta = kalman.X(2);
+ kalman.statelock.unlock();
+
+
+
+ // get IR turret angle --------------------------------------
+ /* float angEst = getAngle(currX, currY); // check this
+ if (angEst != 1000){
+ // here the value should be a valid estimate
+ // so update kalman state
+
+ }
+ */
+ //!!! add code here to update kalman angle state !!!
+
+ // check if target reached ----------------------------------
+ if ( ( abs(currX - targetX) < POSITION_TOR )
+ &&( abs(currY - targetY) < POSITION_TOR )
+ ) {
+
+ diffDir = rectifyAng(currTheta - targetTheta);
+ diffSpeed = diffDir / PI;
+
+ //pc.printf("%f \n",diffDir);
+ if (abs(diffDir) > ANGLE_TOR) {
+ if (abs(diffSpeed) < 0.5) {
+ diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
+ }
+ motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
+
+
+ } else {
+ motors.stop();
+ Thread::wait(4000);
+ //thr_AI.signal_set(0x01);
+ }
+ }
+
+ // adjust motion to reach target ----------------------------
+ else {
+
+ // calc direction to target
+ float targetDir = atan2(targetY - currY, targetX - currX);
+
+
+ diffDir = rectifyAng(currTheta - targetDir);
+ diffSpeed = diffDir / PI;
+
+ if (abs(diffDir) > ANGLE_TOR*2) {
+ if (abs(diffSpeed) < 0.5) {
+ diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
+ }
+ motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
+ } else {
+
+
+ if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) {
+ if (diffSpeed-lastdiffSpeed >= 0) {
+ diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
+ } else {
+ diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
+ }
+ }
+ lastdiffSpeed = diffSpeed;
+
+ // calculte the motor speeds
+ if (diffSpeed <= 0) {
+ speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
+ speedR = MOVE_SPEED;
+
+ } else {
+ speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
+ speedL = MOVE_SPEED;
+ }
+
+ motors.setSpeed( int(speedL), int(speedR));
+ }
+ }
+
+ // wait
+ Thread::wait(MOTION_UPDATE_PERIOD);
+ }
+}
+
+
+void getIRValue(void const *argument) {
+ Thread::signal_wait(0x01);
+ if (IRturret.readable() >= 24) {
+ for (int i=0; i < 24; i++) {
+ IRValues.IR_chars[i] = IRturret.getc();
+ }
+ }
+}
+
+void vIRValueISR (void) {
+ //thr_getIRValue.signal_set(0x01);
+}
+
+
+float getAngle (float x, float y) {
+ /* function that returns current robot orientation
+ input: x, y coords in mm
+ output: orientation in rad
+ note: the angle readings from IR is read in here
+ must be called faster than IR turret frequency (few Hz - currently 3Hz)
+ */
+
+ // variables
+ float ang0 = 0, ang1 = 0, ang2 = 0;
+ int sc0 = 0, sc1 = 0, sc2 = 0;
+
+
+
+ // read serial port
+ if (IRturret.readable()) {
+ IRturret.scanf("%f;%f;%f;%d;%d;%d;",&ang0,&ang1,&ang2,&sc0,&sc1,sc2);
+ }
+
+ // if none sampled
+ if (!sc0 && !sc1 && ! sc2) {
+ return 1000;
+ }
+
+ //
+ else {
+ float est0 = 0, est1 = 0, est2 = 0;
+
+ // calc
+ if ((sc0 > RELI_BOUND_LOW)||(sc0 < RELI_BOUND_HIGH)) {
+ est0 = atan2(1000 - y, 3000-x) - ang0;
+ }
+ if ((sc1 > RELI_BOUND_LOW)||(sc1 < RELI_BOUND_HIGH)) {
+ est1 = atan2( -y, -x) - ang1;
+ }
+ if ((sc2 > RELI_BOUND_LOW)||(sc2 < RELI_BOUND_HIGH)) {
+ est2 = atan2(2000 - y, -x) - ang2;
+ }
+
+ // weighted avg
+ return rectifyAng( ((est0 * RELI_BOUND_HIGH-sc0) + (est1 * RELI_BOUND_HIGH-sc1) + (est2 * RELI_BOUND_HIGH-sc2)) / (3*RELI_BOUND_HIGH - sc0 - sc1 - sc2) );
+ }
+
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/projects/libraries/svn/mbed/trunk@43 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.cpp Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,382 @@
+/**********************************************************
+ * Motors.cpp
+ *
+ * This is a motor control library for the UK1122 L298N based motor controller.
+ * Includes PID controller to control motors speeds
+ *
+ * Author: Crispian Poon
+ * Email: pooncg@gmail.com
+ * Purpose: Eurobot 2012 - ICRS Imperial College London
+ * Date: 4th April 2012
+ * Version: v0.12
+ **********************************************************/
+
+#include "mbed.h"
+#include "motors.h"
+#include "QEI.h" //quadrature encoder library
+#include "globals.h"
+#include "TSH.h"
+
+//*************************************************************************************
+// Constructor
+//*************************************************************************************
+Motors::Motors():
+ current1(p15), current2(p16),
+ Encoder1 (p30, p29, NC, 1856 ,QEI::X4_ENCODING), //connects to motor1 quadracture encoders
+ Encoder2 (p27, p28, NC, 1856 ,QEI::X4_ENCODING), //connects to motor2 quadracture encoders
+ Motor1A(p17), Motor1B(p18), Motor2A(p19), Motor2B(p13), //connects to direction pins
+ Motor1Enable(p25), Motor2Enable(p26), //Connects to control board enable pins to control motors speeds. PWM pins. Remember enable must be set before the direction pins changed!!
+ PIDControllerMotor1(3.5, 0.5, 0, 0.010), PIDControllerMotor2(3.5, 0.5, 0, 0.010) {
+
+//Initialise PID controllers
+ PIDControllerMotor1.setMode(MANUAL_MODE);
+ PIDControllerMotor2.setMode(MANUAL_MODE);
+ PIDControllerMotor1.setBias(-16);
+ PIDControllerMotor2.setBias(-16);
+ PIDControllerMotor1.setOutputLimits(-127, 127);
+ PIDControllerMotor2.setOutputLimits(-127, 127);
+ PIDControllerMotor1.setInputLimits(-102, 102);
+ PIDControllerMotor2.setInputLimits(-102, 102);
+ _lastEncoder1 = 0;
+ _lastEncoder2 = 0;
+
+};
+
+
+//*************************************************************************************
+// Public functions
+//*************************************************************************************
+
+//*********************************************
+//
+// @Description speed regulator task using PID. Run every 10ms.
+//
+//*********************************************
+void Motors::speedRegulatorTask() {
+
+ int latestMotor1Speed = 0;
+ int latestMotor2Speed = 0;
+ int computedSpeed1 = 0;
+ int computedSpeed2 = 0;
+
+ //acceleration control
+ if (accelerationRegister == 1) {
+ if (_accelerationSpeed1 != 0) {
+
+ if (abs(_motorSpeed1) < abs(_accelerationSpeed1)) {
+ _motorSpeed1 += getSignOfInt(_accelerationSpeed1);
+ } else if (abs(_motorSpeed1) > abs(_accelerationSpeed1)) {
+ _motorSpeed1 = _accelerationSpeed1;
+ }
+ }
+ if (_accelerationSpeed2 != 0) {
+ if (abs(_motorSpeed2) < abs(_accelerationSpeed2)) {
+ _motorSpeed2 += getSignOfInt(_accelerationSpeed2);
+ } else if (abs(_motorSpeed2) > abs(_accelerationSpeed2)) {
+ _motorSpeed2 = _accelerationSpeed2;
+ }
+ }
+ }
+
+
+ //MOTOR 1 PID
+ latestMotor1Speed = getEncoder1() - _lastEncoder1; //motor1 encoder change
+ //PID setpoints for 50ms interval.
+ if (_motorSpeed1 == 0) {
+ PIDControllerMotor1.setSetPoint(0);
+ } else {
+ PIDControllerMotor1.setSetPoint((int)(102*((float)_motorSpeed1/127)));
+ }
+ //Process value
+ PIDControllerMotor1.setProcessValue(latestMotor1Speed);
+ //PID Compute
+ computedSpeed1 = (int)PIDControllerMotor1.compute();
+
+
+
+ //MOTOR 2 PID
+ latestMotor2Speed = getEncoder2() - _lastEncoder2; //motor2 encoder change
+ //PID setpoints for 50ms interval.
+ if (_motorSpeed2 == 0) {
+ PIDControllerMotor2.setSetPoint(0);
+ } else {
+ PIDControllerMotor2.setSetPoint((int)(102*((float)_motorSpeed2/127)));
+ }
+ //Process value
+ PIDControllerMotor2.setProcessValue(latestMotor2Speed);
+ //PID Compute
+ computedSpeed2 = (int)PIDControllerMotor2.compute();
+
+
+
+ //debug variables
+ _debug1 = latestMotor1Speed;
+ _debug2 = computedSpeed1;
+
+
+
+ //Set motors speed
+ _setSpeed(computedSpeed1, computedSpeed2);
+
+}
+
+//*********************************************
+//
+// @Description External set speed function for both motors
+// @Parameter [speed] ranges from -127 (revse motor) to 127 (forward motor)
+//
+//*********************************************
+void Motors::setSpeed(int speed) {
+ setSpeed(speed, speed);
+}
+
+//*********************************************
+//
+// @Description External set speed function. Relies on the speedRegulatorTask to change speed.
+// @Parameters [speed1] min -127 (reverse motor), max 127 (forward motor)
+// @Parameters [speed2] min -127 (reverse motor), max 127 (forward motor)
+//
+//*********************************************
+void Motors::setSpeed(int speed1, int speed2) {
+ //set global motor values
+ _motorSpeed1 = speed1;
+ _motorSpeed2 = speed2;
+ _lastEncoder1 = getEncoder1();
+ _lastEncoder2 = getEncoder2();
+
+ //acceleration control
+ if (accelerationRegister == 1) {
+ //target accelerated speed
+ _accelerationSpeed1 = speed1;
+ _accelerationSpeed2 = speed2;
+
+ //current speed
+ _motorSpeed1 = 0;
+ _motorSpeed2 = 0;
+ }
+}
+
+//*********************************************
+//
+// @Description stops motors
+//
+//*********************************************
+void Motors::stop()
+{
+ setSpeed(0);
+}
+
+//*********************************************
+//
+// @Description resets motor1 and motor encoders
+//
+//*********************************************
+void Motors::resetEncoders() {
+ Encoder1.reset();
+ Encoder2.reset();
+}
+
+//*********************************************
+//
+// @Description gets motor1 encoder
+// @returns motor1 encoder counts
+//
+//*********************************************
+int Motors::getEncoder1() {
+ return Encoder1.getPulses();
+}
+
+//*********************************************
+//
+// @Description gets motor2 encoder
+// @returns motor2 encoder counts
+//
+//*********************************************
+int Motors::getEncoder2() {
+ return Encoder2.getPulses();
+}
+
+//*********************************************
+//
+// @Description converts encoder counts to distance in mm
+// @Parameters [encoder] (int) encoder counts
+// @returns distance in mm
+//
+//*********************************************
+int Motors::encoderToDistance(int encoder) {
+ return int((float(encoder) / float(encoderRevCount)) * wheelmm);
+}
+
+//*********************************************
+//
+// @Description converts distance in mm to encoder counts
+// @Parameters [distance] (int) distance in mm
+// @returns encoder counts
+//
+//*********************************************
+int Motors::distanceToEncoder(int distance) {
+ return int((float(distance) / float(wheelmm)) * encoderRevCount);
+}
+
+//*********************************************
+//
+// @Description number sign indicator. determines if number is positive or negative.
+// @Parameters [direction] (int) a number
+// @returns -1 if negative, 1 if positive
+//
+//*********************************************
+int Motors::getSignOfInt(int direction) {
+
+ if (direction > 0) {
+ return 1;
+ } else if (direction < 0) {
+ return -1;
+ }
+ return -1;
+}
+
+//*********************************************
+//Start of quarantined functions
+
+void Motors::move(int distance, int speed) {
+ //resetEncoders(); TODO use kalman as feedback instead!
+
+ int tempEndEncoder = 0;
+ int startEncoderCount = 0;
+
+ tempEndEncoder = distanceToEncoder(abs(distance));
+ startEncoderCount = getEncoder1();
+
+ setSpeed(getSignOfInt(distance) * speed);
+
+ while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+ setSpeed(getSignOfInt(distance) * speed);
+ }
+
+ //resetEncoders();
+ setSpeed(0);
+}
+
+void Motors::turn(int angle, int speed) {
+ //resetEncoders(); TODO use kalman as feedback instead!
+ int tempDistance = int((float(angle) / 360) * float(robotCircumference));
+ int tempEndEncoder = 0;
+ int startEncoderCount = 0;
+
+ tempEndEncoder = distanceToEncoder(abs(tempDistance));
+ startEncoderCount = getEncoder1();
+ setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
+
+ while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+ setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
+
+ }
+
+ //resetEncoders();
+ setSpeed(0);
+}
+
+//Start of quarantined functions
+//*********************************************
+
+
+//*************************************************************************************
+// Private functions
+//*************************************************************************************
+
+//*********************************************
+//
+// @Description internal set speed function
+// @Parameters speed1 min -127, max 127
+// @Parameters speed2 min -127, max 127
+//
+//*********************************************
+void Motors::_setSpeed(int speed1, int speed2) {
+
+//set global encoder values
+ _lastEncoder1 = getEncoder1();
+ _lastEncoder2 = getEncoder2();
+
+//Speed ranges from -127 to 127
+ if (speed1 > 0) {
+ //Motor1 forwards
+ Motor1Enable = (float)speed1/127;
+ Motor1A = 1;
+ Motor1B = 0;
+ //pwm the h bridge driver range 0 to 1 type float.
+
+ } else if (speed1 < 0) {
+ //Motor1 backwards
+ Motor1Enable = (float)abs(speed1)/127;
+ Motor1A = 0;
+ Motor1B = 1;
+
+ } else if (speed1 ==0) {
+ _stop(1,0);
+ }
+
+ if (speed2 > 0) {
+ //Motor2 forwards
+ Motor2Enable = (float)speed2/127;
+
+ Motor2A = 1;
+ Motor2B = 0;
+
+ } else if (speed2 < 0) {
+ //Motor2 backwards
+ Motor2Enable = (float)abs(speed2)/127;
+ Motor2A = 0;
+ Motor2B = 1;
+ } else if (speed2 == 0) {
+ _stop(0,1);
+ }
+
+}
+
+
+//*********************************************
+//
+// @Description stop command for both motors
+//
+//*********************************************
+void Motors::_stop() {
+ _stop(1,1);
+}
+
+//*********************************************
+//
+// @Description stop command for individual motors
+// @Parameter [motor1] stops motor1. =1 is stop. =0 do nothing
+// @Parameter [motor2] stops motor2. =1 is stop. =0 do nothing
+//
+//*********************************************
+void Motors::_stop(int motor1, int motor2) {
+ if (motor1 == 1) {
+ Motor1Enable = 1;
+ Motor1A = 0;
+ Motor1B = 0;
+ }
+
+ if (motor2 == 1) {
+ Motor2Enable = 1;
+ Motor2A = 0;
+ Motor2B = 0;
+ }
+
+}
+
+//*************************************************************************************
+// Redundant functions
+//*************************************************************************************
+
+//Redudant
+void Motors::setMode(int mode) {
+}
+
+//Redudant
+void Motors::sendCommand(char command) {
+}
+
+//Redudant
+void Motors::sendCommand(char command1, char command2 ) {
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,63 @@
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include "mbed.h"
+#include "QEI.h"
+#include "PID.h"
+#include "TSH.h"
+
+class Motors {
+
+public:
+ Motors();
+ Motors(TSI2C &i2cin);
+
+ //Functions declaration
+ void resetEncoders();
+ int getEncoder1();
+ int getEncoder2();
+ void move(int distance, int speed);
+ void turn(int angle, int speed);
+ int getSignOfInt(int direction);
+ void setSpeed(int speed);
+ void setSpeed(int speed1, int speed2);
+ void stop();
+ void setMode(int mode);
+ int encoderToDistance(int encoder);
+ int distanceToEncoder(int distance);
+ void sendCommand(char command);
+ void sendCommand(char command1, char command2 );
+ void speedRegulatorTask();
+ AnalogIn current1;
+ AnalogIn current2;
+ float _debug1;
+ float _debug2;
+ int accelerationRegister; //turns on acceleration control
+
+private:
+
+
+ void _setSpeed(int speed1, int speed2);
+ void _stop();
+ void _stop(int motor1, int motor2);
+ QEI Encoder1;
+ QEI Encoder2;
+ DigitalOut Motor1A;
+ DigitalOut Motor1B;
+ DigitalOut Motor2A;
+ DigitalOut Motor2B;
+ PwmOut Motor1Enable;
+ PwmOut Motor2Enable;
+ int _motorSpeed1;
+ int _motorSpeed2;
+ PID PIDControllerMotor1;
+ PID PIDControllerMotor2;
+ int _lastEncoder1;
+ int _lastEncoder2;
+ int _pidDataBufferIndex;
+ int _accelerationSpeed1;
+ int _accelerationSpeed2;
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rtos.lib Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/projects/libraries-testing/svn/rtos@43 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/system.h Fri Apr 20 20:39:35 2012 +0000
@@ -0,0 +1,49 @@
+
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include "globals.h"
+
+//#include "rtos.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;
+}
+
+#define SIGTICKARGS(thread, signal) \
+ (tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(thread, signal))
+
+typedef void (*tfuncptr_t)(void const *argument);
+
+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;
+};
+
+#endif