p4rec.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "nRF24L01P.h"
00004 #include "motordriver.h"
00005 Serial pc(USBTX, USBRX); // tx, rx
00006  
00007 nRF24L01P NRFR(p5, p6, p7, p8, p9, p10);    // mosi, miso, sck, csn, ce, irq
00008  
00009 //PwmOut led1(LED1); Beware, This will push PWM limits
00010 //PwmOut led2(LED2);
00011 //PwmOut led3(LED3);
00012 //PwmOut led4(LED4);
00013 
00014 Motor  left(p21, p19, p20, 1); // pwm, fwd, rev, has brake feature
00015 Motor right(p22, p17, p18, 1);
00016 
00017 Servo base(p23);
00018 Servo shoulder(p24);
00019 Servo elbow(p25);
00020 Servo claw(p26);
00021 
00022 int main() {
00023  
00024 // The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
00025 //  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
00026 //  only handles 4 byte transfers in the ATMega code.
00027 #define TRANSFER_SIZE   17
00028  
00029     char rxData[TRANSFER_SIZE];
00030     int rxDataCnt = 0;
00031  
00032     NRFR.powerUp();
00033  
00034     // Display the (default) setup of the nRF24L01+ chip
00035     pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  NRFR.getRfFrequency() );
00036     pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  NRFR.getRfOutputPower() );
00037     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", NRFR.getAirDataRate() );
00038     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", NRFR.getTxAddress() );
00039     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", NRFR.getRxAddress() );
00040  
00041     pc.printf( "Recieved Data:\r\n", TRANSFER_SIZE );
00042  
00043     NRFR.setTransferSize( TRANSFER_SIZE );
00044  
00045     NRFR.setReceiveMode();
00046     NRFR.enable();
00047     
00048     int LSL,RSL,VER,HOR;
00049     float FLSL,FRSL,FVER,FHOR;
00050     
00051     base.calibrate(0.0006, 45.0);
00052     shoulder.calibrate(0.0006, 45.0);
00053     elbow.calibrate(0.0010, 45.0);
00054     claw.calibrate(0.0004, 45.0);
00055     float basepos = 0.5, armpos = 0.1;
00056     while (1) {
00057  
00058  
00059         // If we've received anything in the nRF24L01+...
00060         if ( NRFR.readable() ) {
00061             // ...read the data into the receive buffer
00062             rxDataCnt = NRFR.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
00063         if(rxData[16] == 'a')   //error checking
00064         {
00065         LSL =   (rxData[3] << 24) + (rxData[2] << 16) + (rxData[1] << 8) + rxData[0];
00066         FLSL = float(LSL) / 1000;
00067         //led1 = FLSL;
00068         
00069         VER =   (rxData[7] << 24) + (rxData[6] << 16) + (rxData[5] << 8) + rxData[4];
00070         FVER = float(VER) / 1000;
00071         //led2 = FVER;
00072         
00073         HOR =   (rxData[11] << 24) + (rxData[10] << 16) + (rxData[9] << 8) + rxData[8];
00074         FHOR = float(HOR) / 1000;
00075         //led3 = FHOR;
00076         
00077         RSL =   (rxData[15] << 24) + (rxData[14] << 16) + (rxData[13] << 8) + rxData[12];
00078         FRSL = float(RSL) / 1000;
00079         //led4 = FRSL;
00080              
00081         //Output speed
00082         left.speed(-(2*FLSL-1));
00083         right.speed(-(2*FRSL-1));
00084         
00085         //Velocity control
00086         if(basepos > 0.9)
00087             basepos = 0.9;
00088         else if(basepos < 0.1)
00089             basepos = 0.1;
00090         else{
00091             if((FHOR > 0.6)||(FHOR <0.4))
00092                 basepos = basepos + (FHOR -0.5)/1000;}
00093         
00094         if(armpos > 0.9)
00095             armpos = 0.9;
00096         else if(armpos < 0.1)
00097             armpos = 0.1;
00098         else{
00099             if((FVER > 0.6)||(FVER <0.4))
00100                 armpos = armpos + (FVER -0.5)/1000;}
00101         
00102         //output servo
00103         base = basepos;
00104         shoulder = armpos;
00105         elbow = armpos;
00106         claw = armpos;
00107         
00108         }
00109         }
00110     }
00111 }