Reciever Mbed program for "Magician Mobile Robot Arm"
Dependencies: Motordriver Servo mbed nRF24L01P
p4rec.cpp
- Committer:
- bedlou
- Date:
- 2014-10-21
- Revision:
- 0:57045933b470
File content as of revision 0:57045933b470:
#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; } } } }