Louae Tyoan / Mbed 2 deprecated EMBProject4RECV

Reciever Mbed program for "Magician Mobile Robot Arm"

Dependencies:   Motordriver Servo mbed nRF24L01P

p4rec.cpp@0:57045933b470, 2014-10-21 (annotated)

Committer:
bedlou
Date:
Tue Oct 21 16:53:51 2014 +0000
Revision:
0:57045933b470
Reciever Mbed Code for "Magician Mobile Robot Arm"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bedlou 0:57045933b470 1 #include "mbed.h"
bedlou 0:57045933b470 2 #include "Servo.h"
bedlou 0:57045933b470 3 #include "nRF24L01P.h"
bedlou 0:57045933b470 4 #include "motordriver.h"
bedlou 0:57045933b470 5 Serial pc(USBTX, USBRX); // tx, rx
bedlou 0:57045933b470 6
bedlou 0:57045933b470 7 nRF24L01P NRFR(p5, p6, p7, p8, p9, p10); // mosi, miso, sck, csn, ce, irq
bedlou 0:57045933b470 8
bedlou 0:57045933b470 9 //PwmOut led1(LED1); Beware, This will push PWM limits
bedlou 0:57045933b470 10 //PwmOut led2(LED2);
bedlou 0:57045933b470 11 //PwmOut led3(LED3);
bedlou 0:57045933b470 12 //PwmOut led4(LED4);
bedlou 0:57045933b470 13
bedlou 0:57045933b470 14 Motor left(p21, p19, p20, 1); // pwm, fwd, rev, has brake feature
bedlou 0:57045933b470 15 Motor right(p22, p17, p18, 1);
bedlou 0:57045933b470 16
bedlou 0:57045933b470 17 Servo base(p23);
bedlou 0:57045933b470 18 Servo shoulder(p24);
bedlou 0:57045933b470 19 Servo elbow(p25);
bedlou 0:57045933b470 20 Servo claw(p26);
bedlou 0:57045933b470 21
bedlou 0:57045933b470 22 int main() {
bedlou 0:57045933b470 23
bedlou 0:57045933b470 24 // The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
bedlou 0:57045933b470 25 // "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
bedlou 0:57045933b470 26 // only handles 4 byte transfers in the ATMega code.
bedlou 0:57045933b470 27 #define TRANSFER_SIZE 17
bedlou 0:57045933b470 28
bedlou 0:57045933b470 29 char rxData[TRANSFER_SIZE];
bedlou 0:57045933b470 30 int rxDataCnt = 0;
bedlou 0:57045933b470 31
bedlou 0:57045933b470 32 NRFR.powerUp();
bedlou 0:57045933b470 33
bedlou 0:57045933b470 34 // Display the (default) setup of the nRF24L01+ chip
bedlou 0:57045933b470 35 pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", NRFR.getRfFrequency() );
bedlou 0:57045933b470 36 pc.printf( "nRF24L01+ Output power : %d dBm\r\n", NRFR.getRfOutputPower() );
bedlou 0:57045933b470 37 pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", NRFR.getAirDataRate() );
bedlou 0:57045933b470 38 pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", NRFR.getTxAddress() );
bedlou 0:57045933b470 39 pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", NRFR.getRxAddress() );
bedlou 0:57045933b470 40
bedlou 0:57045933b470 41 pc.printf( "Recieved Data:\r\n", TRANSFER_SIZE );
bedlou 0:57045933b470 42
bedlou 0:57045933b470 43 NRFR.setTransferSize( TRANSFER_SIZE );
bedlou 0:57045933b470 44
bedlou 0:57045933b470 45 NRFR.setReceiveMode();
bedlou 0:57045933b470 46 NRFR.enable();
bedlou 0:57045933b470 47
bedlou 0:57045933b470 48 int LSL,RSL,VER,HOR;
bedlou 0:57045933b470 49 float FLSL,FRSL,FVER,FHOR;
bedlou 0:57045933b470 50
bedlou 0:57045933b470 51 base.calibrate(0.0006, 45.0);
bedlou 0:57045933b470 52 shoulder.calibrate(0.0006, 45.0);
bedlou 0:57045933b470 53 elbow.calibrate(0.0010, 45.0);
bedlou 0:57045933b470 54 claw.calibrate(0.0004, 45.0);
bedlou 0:57045933b470 55 float basepos = 0.5, armpos = 0.1;
bedlou 0:57045933b470 56 while (1) {
bedlou 0:57045933b470 57
bedlou 0:57045933b470 58
bedlou 0:57045933b470 59 // If we've received anything in the nRF24L01+...
bedlou 0:57045933b470 60 if ( NRFR.readable() ) {
bedlou 0:57045933b470 61 // ...read the data into the receive buffer
bedlou 0:57045933b470 62 rxDataCnt = NRFR.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
bedlou 0:57045933b470 63 if(rxData[16] == 'a') //error checking
bedlou 0:57045933b470 64 {
bedlou 0:57045933b470 65 LSL = (rxData[3] << 24) + (rxData[2] << 16) + (rxData[1] << 8) + rxData[0];
bedlou 0:57045933b470 66 FLSL = float(LSL) / 1000;
bedlou 0:57045933b470 67 //led1 = FLSL;
bedlou 0:57045933b470 68
bedlou 0:57045933b470 69 VER = (rxData[7] << 24) + (rxData[6] << 16) + (rxData[5] << 8) + rxData[4];
bedlou 0:57045933b470 70 FVER = float(VER) / 1000;
bedlou 0:57045933b470 71 //led2 = FVER;
bedlou 0:57045933b470 72
bedlou 0:57045933b470 73 HOR = (rxData[11] << 24) + (rxData[10] << 16) + (rxData[9] << 8) + rxData[8];
bedlou 0:57045933b470 74 FHOR = float(HOR) / 1000;
bedlou 0:57045933b470 75 //led3 = FHOR;
bedlou 0:57045933b470 76
bedlou 0:57045933b470 77 RSL = (rxData[15] << 24) + (rxData[14] << 16) + (rxData[13] << 8) + rxData[12];
bedlou 0:57045933b470 78 FRSL = float(RSL) / 1000;
bedlou 0:57045933b470 79 //led4 = FRSL;
bedlou 0:57045933b470 80
bedlou 0:57045933b470 81 //Output speed
bedlou 0:57045933b470 82 left.speed(-(2*FLSL-1));
bedlou 0:57045933b470 83 right.speed(-(2*FRSL-1));
bedlou 0:57045933b470 84
bedlou 0:57045933b470 85 //Velocity control
bedlou 0:57045933b470 86 if(basepos > 0.9)
bedlou 0:57045933b470 87 basepos = 0.9;
bedlou 0:57045933b470 88 else if(basepos < 0.1)
bedlou 0:57045933b470 89 basepos = 0.1;
bedlou 0:57045933b470 90 else{
bedlou 0:57045933b470 91 if((FHOR > 0.6)||(FHOR <0.4))
bedlou 0:57045933b470 92 basepos = basepos + (FHOR -0.5)/1000;}
bedlou 0:57045933b470 93
bedlou 0:57045933b470 94 if(armpos > 0.9)
bedlou 0:57045933b470 95 armpos = 0.9;
bedlou 0:57045933b470 96 else if(armpos < 0.1)
bedlou 0:57045933b470 97 armpos = 0.1;
bedlou 0:57045933b470 98 else{
bedlou 0:57045933b470 99 if((FVER > 0.6)||(FVER <0.4))
bedlou 0:57045933b470 100 armpos = armpos + (FVER -0.5)/1000;}
bedlou 0:57045933b470 101
bedlou 0:57045933b470 102 //output servo
bedlou 0:57045933b470 103 base = basepos;
bedlou 0:57045933b470 104 shoulder = armpos;
bedlou 0:57045933b470 105 elbow = armpos;
bedlou 0:57045933b470 106 claw = armpos;
bedlou 0:57045933b470 107
bedlou 0:57045933b470 108 }
bedlou 0:57045933b470 109 }
bedlou 0:57045933b470 110 }
bedlou 0:57045933b470 111 }