Diff: p4rec.cpp

Revision:

0:57045933b470

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/p4rec.cpp	Tue Oct 21 16:53:51 2014 +0000
@@ -0,0 +1,111 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "nRF24L01P.h"
+#include "motordriver.h"
+Serial pc(USBTX, USBRX); // tx, rx
+ 
+nRF24L01P NRFR(p5, p6, p7, p8, p9, p10);    // mosi, miso, sck, csn, ce, irq
+ 
+//PwmOut led1(LED1); Beware, This will push PWM limits
+//PwmOut led2(LED2);
+//PwmOut led3(LED3);
+//PwmOut led4(LED4);
+
+Motor  left(p21, p19, p20, 1); // pwm, fwd, rev, has brake feature
+Motor right(p22, p17, p18, 1);
+
+Servo base(p23);
+Servo shoulder(p24);
+Servo elbow(p25);
+Servo claw(p26);
+
+int main() {
+ 
+// The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
+//  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
+//  only handles 4 byte transfers in the ATMega code.
+#define TRANSFER_SIZE   17
+ 
+    char rxData[TRANSFER_SIZE];
+    int rxDataCnt = 0;
+ 
+    NRFR.powerUp();
+ 
+    // Display the (default) setup of the nRF24L01+ chip
+    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  NRFR.getRfFrequency() );
+    pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  NRFR.getRfOutputPower() );
+    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", NRFR.getAirDataRate() );
+    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", NRFR.getTxAddress() );
+    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", NRFR.getRxAddress() );
+ 
+    pc.printf( "Recieved Data:\r\n", TRANSFER_SIZE );
+ 
+    NRFR.setTransferSize( TRANSFER_SIZE );
+ 
+    NRFR.setReceiveMode();
+    NRFR.enable();
+    
+    int LSL,RSL,VER,HOR;
+    float FLSL,FRSL,FVER,FHOR;
+    
+    base.calibrate(0.0006, 45.0);
+    shoulder.calibrate(0.0006, 45.0);
+    elbow.calibrate(0.0010, 45.0);
+    claw.calibrate(0.0004, 45.0);
+    float basepos = 0.5, armpos = 0.1;
+    while (1) {
+ 
+ 
+        // If we've received anything in the nRF24L01+...
+        if ( NRFR.readable() ) {
+            // ...read the data into the receive buffer
+            rxDataCnt = NRFR.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
+        if(rxData[16] == 'a')   //error checking
+        {
+        LSL =   (rxData[3] << 24) + (rxData[2] << 16) + (rxData[1] << 8) + rxData[0];
+        FLSL = float(LSL) / 1000;
+        //led1 = FLSL;
+        
+        VER =   (rxData[7] << 24) + (rxData[6] << 16) + (rxData[5] << 8) + rxData[4];
+        FVER = float(VER) / 1000;
+        //led2 = FVER;
+        
+        HOR =   (rxData[11] << 24) + (rxData[10] << 16) + (rxData[9] << 8) + rxData[8];
+        FHOR = float(HOR) / 1000;
+        //led3 = FHOR;
+        
+        RSL =   (rxData[15] << 24) + (rxData[14] << 16) + (rxData[13] << 8) + rxData[12];
+        FRSL = float(RSL) / 1000;
+        //led4 = FRSL;
+             
+        //Output speed
+        left.speed(-(2*FLSL-1));
+        right.speed(-(2*FRSL-1));
+        
+        //Velocity control
+        if(basepos > 0.9)
+            basepos = 0.9;
+        else if(basepos < 0.1)
+            basepos = 0.1;
+        else{
+            if((FHOR > 0.6)||(FHOR <0.4))
+                basepos = basepos + (FHOR -0.5)/1000;}
+        
+        if(armpos > 0.9)
+            armpos = 0.9;
+        else if(armpos < 0.1)
+            armpos = 0.1;
+        else{
+            if((FVER > 0.6)||(FVER <0.4))
+                armpos = armpos + (FVER -0.5)/1000;}
+        
+        //output servo
+        base = basepos;
+        shoulder = armpos;
+        elbow = armpos;
+        claw = armpos;
+        
+        }
+        }
+    }
+}
\ No newline at end of file