rc.cpp

00001 #include "mbed.h"
00002 #include "MRF24J40.h"
00003 #include "sender.h"
00004 #include "analog_joystick.h"
00005 
00006 #include <string.h>
00007 
00008 // RF tranceiver to link with handheld.
00009 MRF24J40 mrf(p11, p12, p13, p14, p21);
00010 
00011 // LEDs you can treat these as variables (led2 = 1 will turn led2 on!)
00012 DigitalOut led1(LED1);
00013 DigitalOut led2(LED2);
00014 DigitalOut led3(LED3);
00015 DigitalOut led4(LED4);
00016 
00017 // Timer
00018 Timer timer;
00019 
00020 // Serial port for showing RX data.
00021 Serial pc(USBTX, USBRX);
00022 
00023 int main (void)
00024 {
00025     long long id = 0;
00026     uint8_t channel = 3;
00027 
00028     //Set the Channel. 0 is default, 15 is max
00029     mrf.SetChannel(channel);
00030 
00031     //Start the timer
00032     timer.start();
00033 
00034     pc.printf("START\r\n");
00035 
00036     char txBuffer[250];
00037 
00038 
00039     float thrustOffset =0;
00040     float rollOffset =0;
00041     float pitchOffset =0;
00042     float yawOffset =0;
00043 
00044     int nSamples = 100;
00045 
00046     for (int n = 0; n < nSamples; n++) {
00047         thrustOffset += read_thrust();
00048         yawOffset += read_yaw();
00049         pitchOffset += read_pitch();
00050         rollOffset += read_roll();
00051 
00052     }
00053     thrustOffset /= nSamples;
00054     yawOffset /= nSamples;
00055     pitchOffset /= nSamples;
00056     rollOffset /= nSamples;
00057 
00058 
00059     while(1) {
00060         sprintf(txBuffer, "%lld,%f,%f,%f,%f", id, read_thrust()-thrustOffset, read_yaw()-yawOffset, read_pitch()-pitchOffset, read_roll()-rollOffset);
00061         rf_send(mrf, txBuffer, strlen(txBuffer) + 1);
00062         pc.printf("RC Sent: %s \r\n", txBuffer);
00063 
00064         id++;
00065     }
00066 }