Remote Ctrl

Dependencies:   mbed nRF24L01P

main.cpp

Committer:
Ucial
Date:
2019-12-09
Revision:
0:61c20540a36f

File content as of revision 0:61c20540a36f:

#include "mbed.h"
#include "nRF24L01P.h"

uint8_t txdata[32] ;
float   L_X;
float   L_Y;
float   R_X;
float   R_Y;
int     Yaw;
int     Roll;
int     Pitch;
int     Power;
int     ii;
int     SendCount;

Ticker      tick;
nRF24L01P   nrf24_PB_15(PB_15,PB_14,PB_13,PB_6,PB_5,PB_7);
AnalogIn    myIOPA_0(PA_0);
AnalogIn    myIOPA_1(PA_1);
AnalogIn    myIOPB_0(PB_0);
AnalogIn    myIOPB_1(PB_1);

uint8_t Get_Checksum(uint8_t mydata[])
{   
    uint8_t i;
    uint8_t checksum=0;
    uint8_t length = mydata[3];

    for(i=0;i<length+2;i++)
        checksum ^= (mydata[3+i]&0xFF);
    return checksum;
}

void tickhandle()
{
    //500Hz task
    SendCount = SendCount + 1;
    nrf24_PB_15.NRF24L01_IRQ();
    if(SendCount%5==0)
    {
        //100Hz task
        nrf24_PB_15.NRF_Send_TX(txdata,32);
    }
}

int main() 
{

    for(ii = 1; ii <= 32; ii++)
        txdata[(ii - 1)] = 0;
    txdata[0] = 36;
    txdata[1] = 77;
    txdata[2] = 60;
    txdata[3] = 16;
    txdata[4] = 105;
    txdata[13]=220;
    txdata[14]=5;
    txdata[15]=220;
    txdata[16]=5;
    txdata[17]=220;
    txdata[18]=5;
    txdata[19]=0;
    txdata[20]=0;
    
    SendCount = 0;
    L_X=0.0;
    L_Y=0.0;
    R_X=0.0;
    R_Y=0.0;
    
    tick.attach(&tickhandle,0.002);
    while (true)
    {
        //Paste your code here(Ctrl+V)
        L_X = myIOPA_0.read();
        L_Y = myIOPA_1.read();
        R_X = myIOPB_0.read();
        R_Y = myIOPB_1.read();
        
        Power = 2000-1000*L_Y;
        Yaw =   2000-1000*L_X;
        Pitch = 2000-1000*R_Y;
        Roll =  2000-1000*R_X;
        
        txdata[5] = Roll % 256;
        txdata[6] = Roll / 256;
        txdata[7] = Pitch % 256;
        txdata[8] = Pitch / 256;
        txdata[9] = Yaw % 256;
        txdata[10] = Yaw / 256;
        txdata[11] = Power % 256;
        txdata[12] = Power / 256;

        txdata[21] = Get_Checksum(txdata);
        if(SendCount>500)
        {
            SendCount=0;
        }
    }
}