Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Revision 9:377560539b74, committed 2012-04-28
- Comitter:
- narshu
- Date:
- Sat Apr 28 17:21:24 2012 +0000
- Parent:
- 8:ffc7d8af2d5a
- Child:
- 10:294b9adbc9d3
- Commit message:
- Restructured project to have a single shared lib; Also raised the RF baud rate
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/.lib Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,1 @@ + \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/Kalman/IR/IR.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,74 @@ +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/Kalman/IR/IR.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,25 @@ + +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/Kalman/Kalman.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,404 @@ +//*************************************************************************************** +//Kalman Filter implementation +//*************************************************************************************** +#include "Kalman.h" +#include "rtos.h" +#include "RFSRF05.h" +#include "math.h" +#include "globals.h" +#include "motors.h" +#include "system.h" +#include "geometryfuncs.h" + +#include <tvmet/Matrix.h> +#include <tvmet/Vector.h> +using namespace tvmet; + +Kalman::Kalman(Motors &motorsin, + 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), + motors(motorsin), + 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; + X = 0.5, 0, 0; + + 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 + wait(1); + + //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 y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); + float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; + + 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); + // 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); + + //statelock already locked + X(0) = x_coor/1000.0f; + X(1) = y_coor/1000.0f; + X(2) = 0; + + // 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 = motors.getEncoder1(); + int rightenc = motors.getEncoder2(); + + float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; + float dright = motors.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(); + + //Linearising F around X + Matrix<float, 3, 3> F; + F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))), + 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))), + 0, 0, 1; + + //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); + + //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); + } + + } + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/Kalman/Kalman.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,97 @@ +#ifndef KALMAN_H +#define KALMAN_H + +#include "globals.h" + + +#include "rtos.h" +//#include "Matrix.h" +#include "motors.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(Motors &motorsin, + 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(); + +private: + + //Sensor interfaces + RFSRF05 sonararray; + Motors& motors; + 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/Kalman/Sonar/RF12B/RF12B.cpp Sat Apr 28 17:21:24 2012 +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/Eurobot_shared/Kalman/Sonar/RF12B/RF12B.h Sat Apr 28 17:21:24 2012 +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/Eurobot_shared/Kalman/Sonar/RF12B/RF_defs.h Sat Apr 28 17:21:24 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/Eurobot_shared/Kalman/Sonar/RFSRF05.cpp Sat Apr 28 17:21:24 2012 +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 330, we put 400 so circles overlap + _dist[_beacon_counter] = _timer.read_us()/2.9 + 400; + + 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/Eurobot_shared/Kalman/Sonar/RFSRF05.h Sat Apr 28 17:21:24 2012 +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/Eurobot_shared/TSH.h Sat Apr 28 17:21:24 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/Eurobot_shared/ai/ai.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,32 @@ + +#include "ai.h" +#include "rtos.h" +#include "globals.h" + +AI::AI() : + thr_AI(aithreadwrapper,this,osPriorityNormal,1024) { + flag_terminate = false; + //printf("aistart\r\n"); +} + +void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing) { + targetlock.lock(); + target.x = targetX; + target.y = targetY; + target.theta = targetTheta; + target.facing = targetfacing; + targetlock.unlock(); +} + +void AI::settarget(Target targetin) { + targetlock.lock(); + target = targetin; + targetlock.unlock(); +} + +AI::Target AI::gettarget() { + targetlock.lock(); + Target temptarget = target; + targetlock.unlock(); + return temptarget; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/ai/ai.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,33 @@ +#ifndef AI_H +#define AI_H + +#include "rtos.h" + +class AI { +public: +AI(); + +Mutex targetlock; +Thread thr_AI; + +struct Target { + float x; + float y; + float theta; + bool facing; +} target; + +void settarget(float targetX, float targetY, float targetTheta, bool targetfacing = true); +void settarget(Target); +Target gettarget(); + +bool flag_terminate;// = false; + +private: + +void ai_thread (); +static void aithreadwrapper(void const *arg){ ((AI*)arg)->ai_thread(); } + +}; + +#endif //AI_H \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/geometryfuncs/geometryfuncs.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,28 @@ +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/system/system.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,29 @@ +#include "system.h" + +//Defining the externs +DigitalOut OLED1(LED1); +DigitalOut OLED2(LED2); +DigitalOut OLED3(LED3); +DigitalOut OLED4(LED4); + +//nop style wait function +void nopwait(int ms){ +while(ms--) + for (volatile int i = 0; i < 24000; i++); +} + +float cpupercent; //defining the extern +void measureCPUidle (void const* arg) { + + Timer timer; + cpupercent = 0; //defined in system.h + + while(1) { + timer.reset(); + timer.start(); + wait(1); + + int thistime = timer.read_us()-1000000; + cpupercent = thistime; + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/system/system.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,51 @@ + +#ifndef SYSTEM_H +#define SYSTEM_H + +#include "globals.h" +#include "rtos.h" + +//Declaring the onboard LED's for everyone to use +extern DigitalOut OLED1;//(LED1); +extern DigitalOut OLED2;//(LED2); +extern DigitalOut OLED3;//(LED3); +extern DigitalOut OLED4;//(LED4); + +//nop style wait function +void nopwait(int ms); + +//a type which is a pointer to a rtos thread function +typedef void (*tfuncptr_t)(void const *argument); + +//--------------------- +//Signal ticker stuff +#define SIGTICKARGS(thread, signal) \ + (tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(thread, signal)) + +class Signalsetter { +public: + Signalsetter(Thread& inthread, int insignal) : + thread(inthread) { + signal = insignal; + //pc.printf("ptr saved as %#x \r\n", (int)(&(inthread))); + } + + static void callback(void* thisin) { + + Signalsetter* fthis = (Signalsetter*)thisin; + //pc.printf("callback will signal thread object at %#x \r\n", (int)(&(fthis->thread))); + fthis->thread.signal_set(fthis->signal); + //delete fthis; //this is useful for single fire tickers! + } + +private: + Thread& thread; + int signal; +}; + +//--------------------- +//cpu usage measurement function +extern float cpupercent; +void measureCPUidle (void const* arg); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/ui/ui.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,99 @@ + +#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() { + + Thread::wait(1500); + + 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); + } + +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eurobot_shared/ui/ui.h Sat Apr 28 17:21:24 2012 +0000 @@ -0,0 +1,30 @@ + +#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
--- a/Kalman/.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/narshu/libraries/Kalman/m93uhf \ No newline at end of file
--- a/Kalman/IR/IR.cpp Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,86 +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) { - - //Starting values of IR calibration - angleInit = false; - angleOffset = 0; - - //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 - - if (angleInit) { - kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]); - } else { - //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix - //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]); - - //only update the IRMeasures used by kalman init - kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1]; - } - - } - - } -} \ No newline at end of file
--- a/Kalman/IR/IR.h Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,28 +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; - - bool angleInit; // = false; - float angleOffset; // = 0; - - 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 Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,380 +0,0 @@ -//*************************************************************************************** -//Kalman Filter implementation -//*************************************************************************************** -#include "Kalman.h" -#include "rtos.h" -#include "RFSRF05.h" -#include "math.h" -#include "globals.h" -#include "motors.h" -#include "system.h" -#include "geometryfuncs.h" - -#include <tvmet/Matrix.h> -#include <tvmet/Vector.h> -using namespace tvmet; - -Kalman::Kalman(Motors &motorsin, - 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), - motors(motorsin), - 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) { - - Kalman_init = false; - //Intialising some arrays to zero - for (int kk = 0; kk < 3; kk ++) { - SonarMeasure_Offset[kk] = 0; - } - //Initialising other vars - - - //Initilising matrices - - // X = x, y, theta; - X = 0.5, 0, 0; - - 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; - // set initiating flag to false - Kalman_init = false; - - // init the offset array - for (int k = 0; k < 3; k ++) { - SonarMeasure_Offset[k] = 0; - IRMeasures[k] = 0; - } - -/* -#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 -*/ - - ir.attachisr(); - //wating untill the IR has reved up and picked up some data - wait(1); - - //temporaraly disable IR updates - ir.detachisr(); - - //lock the state throughout the computation, as we will override the state at the end - 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 y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); - float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; - - //Compute sonar offset - float Dist_Exp[3]; - for (int k = 0; k < 3; k++) { - Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor)); - SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f; - } - - //Compute IR offset - ir.angleOffset = 0; - for (int i = 0; i < 3; i++) { - float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); - // 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.angleOffset += angle_temp; - } - } - ir.angleOffset = ir.angleOffset/float(beacon_cnt); - //printf("\n\r"); - - //statelock already locked - ir.angleInit = true; - // set int flag to true - Kalman_init = true; - X(0) = x_coor/1000.0f; - X(1) = y_coor/1000.0f; - X(2) = 0; - statelock.unlock(); - - //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI); - - //reattach the IR processing - ir.attachisr(); - //IRturret.attach(&IR::vIRValueISR,Serial::RxIrq); -} - - -void Kalman::predictloop() { - - OLED4 = !ui.regid(0, 3); - OLED4 = !ui.regid(1, 9); - - float lastleft = 0; - float lastright = 0; - - while (1) { - Thread::signal_wait(0x1); - OLED1 = !OLED1; - - int leftenc = motors.getEncoder1(); - int rightenc = motors.getEncoder2(); - - float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; - float dright = motors.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(); - - //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 - Matrix<float, 3, 3> F; - F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))), - 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))), - 0, 0, 1; - - //Generating forward and rotational variance - float varfwd = fwdvarperunit * (dright + dleft) / 2.0f; - float varang = varperang * thetap; - float varxydt = xyvarpertime * PREDICTPERIOD; - float varangdt = angvarpertime * PREDICTPERIOD; - - //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)}; - OLED4 = !ui.updateval(0, statecpy, 3) || OLED4; - - float Pcpy[] = {P(0,0), P(0,1), P(0,2), P(1,0), P(1,1), P(1,2), P(2,0), P(2,1), P(2,2)}; - OLED4 = !ui.updateval(1, Pcpy, 9) || OLED4; - - statelock.unlock(); - } -} - -//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() { - 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) { - - float dist = value / 1000.0f; //converting to m from mm - int sonarid = type; - aborton2stddev = false; - - // Remove the offset if possible - if (Kalman_init) - dist = dist - SonarMeasure_Offset[sonarid]; - - 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; - - dhdx = rbx / expecdist; - dhdy = rby / expecdist; - - H = dhdx, dhdy, 0; - - } else if (type <= IR3) { - - aborton2stddev = false; - 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(-rby, -rbx) - 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 { - OLED4 = 1; - //printf("ERROR: in updateloop, code %#x", evt); - } - - } - -} \ No newline at end of file
--- a/Kalman/Kalman.h Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,95 +0,0 @@ -#ifndef KALMAN_H -#define KALMAN_H - -#include "globals.h" - - -#include "rtos.h" -//#include "Matrix.h" -#include "motors.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(Motors &motorsin, - 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 SonarMeasure_Offset[3]; - - 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(); - -private: - - //Sensor interfaces - RFSRF05 sonararray; - Motors& motors; - 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 Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,395 +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); - - - // 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; - static unsigned char temp; - - //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 = data&0x00FF; - //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 Fri Apr 27 18:36:54 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 Fri Apr 27 18:36:54 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 Fri Apr 27 18:36:54 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 330, we put 400 so circles overlap - _dist[_beacon_counter] = _timer.read_us()/2.9 + 400; - - 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 Fri Apr 27 18:36:54 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
--- a/TSH.h Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,62 +0,0 @@ -#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
--- a/ai/.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/narshu/libraries/ai/m94fo9 \ No newline at end of file
--- a/ai/ai.cpp Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,83 +0,0 @@ - -#include "ai.h" -#include "rtos.h" -#include "globals.h" - -AI::AI() : - thr_AI(aithreadwrapper,this,osPriorityNormal,1024) { - flag_terminate = false; - //printf("aistart\r\n"); -} - -void AI::settarget(float targetX, float targetY, float targetTheta, bool targetfacing){ - targetlock.lock(); - target.x = targetX; - target.y = targetY; - target.theta = targetTheta; - target.facing = targetfacing; - targetlock.unlock(); -} - -void AI::settarget(Target targetin){ - targetlock.lock(); - target = targetin; - targetlock.unlock(); -} - -AI::Target AI::gettarget(){ - targetlock.lock(); - Target temptarget = target; - targetlock.unlock(); - return temptarget; -} - -void AI::ai_thread () { - /* - //printf("aithreadstart\r\n"); - Thread::signal_wait(0x01); - settarget(660, 400, PI/2, true); - - Thread::signal_wait(0x01); - settarget(660, 570, PI, true); - - Thread::signal_wait(0x01); - settarget(400, 870, PI, true); - - Thread::signal_wait(0x01); - settarget(660, 870, PI, false); - - flag_terminate = true; - */ - - // goes to the mid - Thread::signal_wait(0x01); - settarget(1500, 1000, PI/2, true); - - // left roll - Thread::signal_wait(0x01); - settarget(500, 1700, PI/2, true); - - // mid - Thread::signal_wait(0x01); - settarget(1500, 1000, PI/2, true); - - // map - Thread::signal_wait(0x01); - settarget(1500, 1700, PI/2, true); - - // mid - Thread::signal_wait(0x01); - settarget(1500, 1000, -PI/2, true); - - // home - Thread::signal_wait(0x01); - settarget(500, 500, 0, true); - - Thread::signal_wait(0x01); - flag_terminate = true; - //OLED3 = true; - - while (true) { - Thread::wait(osWaitForever); - } -} \ No newline at end of file
--- a/ai/ai.h Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,33 +0,0 @@ -#ifndef AI_H -#define AI_H - -#include "rtos.h" - -class AI { -public: -AI(); - -Mutex targetlock; -Thread thr_AI; - -struct Target { - float x; - float y; - float theta; - bool facing; -} target; - -void settarget(float targetX, float targetY, float targetTheta, bool targetfacing = true); -void settarget(Target); -Target gettarget(); - -bool flag_terminate;// = false; - -private: - -void ai_thread (); -static void aithreadwrapper(void const *arg){ ((AI*)arg)->ai_thread(); } - -}; - -#endif //AI_H \ No newline at end of file
--- a/geometryfuncs.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/narshu/libraries/geometryfuncs/m943ji \ No newline at end of file
--- a/globals.h Fri Apr 27 18:36:54 2012 +0000 +++ b/globals.h Sat Apr 28 17:21:24 2012 +0000 @@ -1,42 +1,44 @@ #ifndef GLOBALS_H #define GLOBALS_H +#include "mbed.h" +#define PI 3.14159265 + //#define ROBOT_SECONDARY -#ifndef ROBOT_SECONDARY -#define ROBOT_PRIMARY -// invert echo polarity for primary -#define SONAR_ECHO_INV -// Primary Robot constants -const int robot_width = 410; -const int encoderRevCount = 1856; -const int wheelmm = 314; -const int robotCircumference = 1256; -#else +#ifdef ROBOT_SECONDARY //Secondary Robot constants in mm const int robot_width = 260; const int encoderRevCount = 360; const int wheelmm = 226; const int robotCircumference = 816; -#endif - -#include "mbed.h" - -#define PI 3.14159265 +#else +#define ROBOT_PRIMARY +// invert echo polarity for primary +#define SONAR_ECHO_INV +// Primary Robot constants +const int robot_width = 390; +const int encoderRevCount = 1856; +const int wheelmm = 304; +const int robotCircumference = 1150; +#endif //Robot movement constants -const float fwdvarperunit = 0.005; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN! -const float varperang = 3E-5; //around 1 degree stddev per 180 turn -const float xyvarpertime = 0.001; //(very poorly) accounts for hitting things +const float fwdvarperunit = 0.008; //1 std dev = 7% //NEEDS TO BE MEASURED AGAIN! +const float varperang = 0.005; //around 1 degree stddev per 180 turn +const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things const float angvarpertime = 0.001; //sonar constants static const float sonarvariance = 0.005; +//IR constants +static const float IRvariance = 0.001; + //Arena constants struct pos { int x; @@ -64,7 +66,7 @@ #define ANGLE_TOR 0.15 // motion control -#define MOVE_SPEED 30 +#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
--- a/main.cpp Fri Apr 27 18:36:54 2012 +0000 +++ b/main.cpp Sat Apr 28 17:21:24 2012 +0000 @@ -40,10 +40,10 @@ //Init kalman kalman.KalmanInit(); - Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); + //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); - //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024); + Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024); //Motion_Thread_Ptr = &thr_motion; //measure cpu usage. output updated once per second to symbol cpupercent @@ -53,11 +53,80 @@ //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! while (1) { + + osThreadSetPriority (osThreadGetId(), osPriorityIdle); + + Timer timer; + ui.regid(10, 1); + + while(1) { + timer.reset(); + timer.start(); + nopwait(1000); + + ui.updateval(10, timer.read_us()); + } + // do nothing Thread::wait(osWaitForever); } } +void AI::ai_thread () { + /* + //printf("aithreadstart\r\n"); + Thread::signal_wait(0x01); + settarget(660, 400, PI/2, true); + + Thread::signal_wait(0x01); + settarget(660, 570, PI, true); + + Thread::signal_wait(0x01); + settarget(400, 870, PI, true); + + Thread::signal_wait(0x01); + settarget(660, 870, PI, false); + + flag_terminate = true; + */ + + while (1) { + + // goes to the mid + Thread::signal_wait(0x01); + settarget(1500, 1000, PI/2, true); + + // left roll + Thread::signal_wait(0x01); + settarget(500, 1500, PI/2, true); + + // mid + Thread::signal_wait(0x01); + settarget(1500, 1000, PI/2, true); + + // map + Thread::signal_wait(0x01); + settarget(1500, 1500, PI/2, true); + + // mid + Thread::signal_wait(0x01); + settarget(1500, 1000, -PI/2, true); + + // home + Thread::signal_wait(0x01); + settarget(500, 500, 0, true); + + } + + Thread::signal_wait(0x01); + flag_terminate = true; + //OLED3 = true; + + while (true) { + Thread::wait(osWaitForever); + } +} + void vMotorThread(void const *argument) { motors.resetEncoders(); @@ -105,7 +174,6 @@ pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]); pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI); - pc.printf("Angle_Offset: %0.4f \r\n",kalman.ir.angleOffset*180/PI); Thread::wait(100); } }
--- a/motors/.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ - \ No newline at end of file
--- a/motors/motors.cpp Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,384 +0,0 @@ -/********************************************************** - * 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(): - 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; - - //speed regulator task using PID. Run every 10ms. - _ticker.attach(this, &Motors::speedRegulatorTask, 0.01); - -}; - - -//************************************************************************************* -// 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
--- a/motors/motors.h Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,62 +0,0 @@ -#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(); - 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; - Ticker _ticker; - -}; - -#endif
--- a/system.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/narshu/libraries/system/m92heu \ No newline at end of file
--- a/ui/.lib Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/narshu/libraries/ui/m93vvw \ No newline at end of file
--- a/ui/ui.cpp Fri Apr 27 18:36:54 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,99 +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() { - - Thread::wait(1500); - - 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 Fri Apr 27 18:36:54 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