![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Differential Drive Robot Controlled by nRF24l01+ radio module.
Dependencies: mbed mbed-rtos QEI nRF24L01P-mbed
Diff: main.cpp
- Revision:
- 0:d5c0d6f47dd1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 08 13:36:16 2019 +0000 @@ -0,0 +1,354 @@ +#include "mbed.h" +#include "nRF24L01P.h" +#include "rtos.h" +#include "Serial.h" +#include "QEI.h" + +//Function Prototypes +void Encoder_ISR(void); //Handler for Encoder +void EncoderThread(void const *argument); //Handler for Encoder +void PID1(void const *argument); //PID Control for motor 1 +void PID2(void const *argument); //PID Control for motor 2 +void PeriodicInterruptISR(void); //Periodic Timer Interrupt Serive Routine for Motor 1 +void PeriodicInterrupt2ISR(void); //Periodic Timer Interrupt Serive Routine for Motor 2 +//void AssignData(); + +//Processes and Threads +osThreadId EncoderID, Encoder2ID, PiControlId, PiControl2Id; // Thread ID's +osThreadDef(PID1, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority +osThreadDef(PID2, osPriorityRealtime, 1024); // Declare PiControlThread2 as a thread, highest priority +osThreadDef(EncoderThread, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority + +Serial pc(USBTX, USBRX); // tx, rx +nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTD0, PTE0, PTD5); // mosi, miso, sck, csn, ce, irq +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); + +// Data Packets Requirements +#define StartBit 0xFF +#define IDbit 0x01 +#define EndBit 0xFF + +#define TRANSFER_SIZE 8 + +char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE]; +int txDataCnt = 0; +int rxDataCnt = 0; + + +//Port Configuration +//Motor Directions +DigitalOut dir_motor1(PTE4); //Direction Motor 1 +DigitalOut dir_motor2(PTE5); //Direction Motor 2 + +//PWM Outputs +PwmOut PWM_motor1(PTE20); //PWM Motor2 +PwmOut PWM_motor2(PTE22); //PWM Motor1 + +//Kick Output +DigitalOut kick(PTB10); //Kick command +//Encoder +QEI motor1(PTD4, PTA12, NC, 16, QEI::X4_ENCODING); //QEI for motor 1 +QEI motor2(PTA4, PTA5, NC, 16, QEI::X4_ENCODING); //QEI for motor 2 +//Timer Interrupts +Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt +Ticker PeriodicInt2; // Declare a timer interrupt: PeriodicInt2 +Ticker Encoder_Tick; //Timer Interrupt for counting Pulses + + + + +//Declare Global Variables +float des_vel = 0, mes_vel = 0, des_vel2 = 0, mes_vel2 = 0; +float e = 0, e_Prev = 0, e2 = 0, e_Prev2 = 0; +float I_Term = 0, I_Term2 = 0, D_Term = 0, D_Term2 = 0; +float Kp = 5, Kd = 5, Ki = 1.05; //Current Best is Kp = 5, Kd = 5, Ki = 1.05, Prev Best is Kp = 5, Kd = 5, Ki = 1.1, this is for step input +float u = 0, u2 = 0; +float r = 50, r2 = 50; +float ramp_start = 50, ramp_end = 260, ramp_rate = 8, ramp_rate2 = 8; //Ramp rate for motors could be as low as 0.2 or as high as 410? +float dt = 0.05; //dt best = 0.05? +float pulses_motor1, pulses_motor2; +float PWM_Period = 2000; //in micro-seconds + + +//Testing Digital Input for LED +//DigitalOut led3(LED3); + + +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. + pc.baud(9600); // Set max uart baud rate + pc.printf("We have good serial communication! :D\n"); + + //Initializing Threads for interrupts + PiControlId = osThreadCreate(osThread(PID1), NULL); //Create Thread for PID1 + PiControl2Id = osThreadCreate(osThread(PID2), NULL); //Create Thread for PID2 + EncoderID = osThreadCreate(osThread(EncoderThread), NULL); //Create Encoder Thread + + //PWM Period + PWM_motor1.period_us(PWM_Period); // This sets the PWM period to 2000 us = 500Hz + PWM_motor2.period_us(PWM_Period); // This sets the PWM period to 2000 us = 500Hz + + //Periodic Interrupts + PeriodicInt.attach(&PeriodicInterruptISR, dt); //Periodic timer interrupt every 0.02 seconds + PeriodicInt2.attach(&PeriodicInterrupt2ISR, dt); //Periodic timer interrupt every 0.02 seconds + Encoder_Tick.attach(&Encoder_ISR, 4*dt); + + my_nrf24l01p.powerUp(); + my_nrf24l01p.setAirDataRate(NRF24L01P_DATARATE_2_MBPS); + + // Display the (default) setup of the nRF24L01+ chip + pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() ); + pc.printf( "nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() ); + pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() ); + pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() ); + pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() ); + + pc.printf( "Type keys to test transfers:\r\n (transfers are grouped into %d characters)\r\n", TRANSFER_SIZE ); + + my_nrf24l01p.setTransferSize( TRANSFER_SIZE ); + + my_nrf24l01p.setReceiveMode(); + my_nrf24l01p.enable(); + + + kick=0; + + char test = 0; + //Loop Forever + while (1) { + /* + // If we've received anything over the host serial link... + if ( pc.readable() ) { + + // ...add it to the transmit buffer + txData[txDataCnt++] = pc.getc(); + + // If the transmit buffer is full + if ( txDataCnt >= sizeof( txData ) ) { + + // Send the transmitbuffer via the nRF24L01+ + my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt ); + + txDataCnt = 0; + } + + // Toggle LED1 (to help debug Host -> nRF24L01+ communication) + myled1 = !myled1; + }*/ + + // If we've received anything in the nRF24L01+... + if ( my_nrf24l01p.readable() ) { + + // ...read the data into the receive buffer + rxDataCnt = my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) ); + if((rxData[0] == StartBit) && (rxData[1] == IDbit) && (rxData[7] == EndBit)){ + // Transmitted Data + des_vel = rxData[2]; + dir_motor1=rxData[3]; + des_vel2 = rxData[4]; + dir_motor2=rxData[5]; + kick = rxData[6]; + // led3 = !led3; + } + + // AssignData(); + // Display the receive buffer contents via the host serial link + /*for ( int i = 0; rxDataCnt > 0; rxDataCnt--, i++ ) { + + pc.putc( rxData[i] ); + }*/ + pc.putc('\n)'); + pc.putc('\r'); + pc.putc(rxData[0]); + pc.putc(rxData[1]); + pc.putc(rxData[2]); + pc.putc(rxData[3]); + pc.putc(rxData[4]); + pc.putc(rxData[5]); + pc.putc(rxData[6]); + pc.putc(rxData[7]); + pc.putc('\n'); + pc.putc('\r'); + + // Toggle LED2 (to help debug nRF24L01+ -> Host communication) + myled2 = !myled2; + } + } +} + + + +/* +void AssignData(){ + if((rxData[0] == StartBit) && (rxData[1] == IDbit) && (rxData[7] == EndBit)){ + // Transmitted Data + des_vel = rxData[2]; + dir_motor1=rxData[3]; + des_vel2 = rxData[4]; + dir_motor2=rxData[5]; + kick = rxData[6]; + // led3 = !led3; + } +} + +*/ + +// ******** Encoder Thread ******** +void EncoderThread(void const *argument){ + while(true){ + osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received + + pulses_motor1 = abs(motor1.getPulses()); //Get number of pulses from motor1 + pulses_motor2 = abs(motor2.getPulses()); //Get number of pulses from motor2 + } +} + +// ******** PID1 Thread ******** +void PID1(void const *argument){ + + while(true){ + osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. + //dir_motor1 = 1; + //pulses_motor1 = abs(motor1.getPulses()); //Get number of pulses from motor1 + //des_vel = 30; + mes_vel = (60*pulses_motor1) / (700 * dt * 4); //Calculate the measured speed + + if(pulses_motor1 == 0){ + mes_vel = 0; //Accounting for the case of having desired velocity equal to zero + } + + if(r > ramp_end){ + r = ramp_end; + } + + //Ramping + if(des_vel <= 50){ + ramp_rate = 8; //Default ramp rate + } + else{ + ramp_rate = des_vel/75; //Faster Ramp Rate + } + + if(r < des_vel){ + r = r + ramp_rate; //Ramp up + + if(r > des_vel){ + r = des_vel; //Limit if it over shoots the desired + } + } + if(r > des_vel){ + r = r - ramp_rate; //Ramp down + + if(r < des_vel){ + r = des_vel; //Limit if it under shoots the desired + } + } + + if(mes_vel < 0){ + mes_vel = -mes_vel; //Ensure Positive Feedback + } + e = r - mes_vel; //Error + D_Term = (e - e_Prev) / (dt); //Slope + u = abs(e*Kp + I_Term*Ki + D_Term*Kd); + + if(abs(u) >= PWM_Period){ + u = PWM_Period;//Max Duty Cycle + } + else{ + if(des_vel != 0 && mes_vel != 0){ + I_Term = I_Term + e*dt; //Previous error + error*time step, scaling factor 10e2 + } + } + + PWM_motor1.pulsewidth_us(abs(u)); //Set the Pulsewidth output + e_Prev = e; //Previous Error + motor1.reset(); //Reset the motor1 encoder + } + +} + +// ******** PID2 Thread ******** +void PID2(void const *argument){ + + while(true){ + osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. + + //dir_motor2 = 1; + //pulses_motor2 = abs(motor2.getPulses()); //Get number of pulses from motor2 + //des_vel2 = 20; + mes_vel2 = (60*pulses_motor2) / (700 * dt * 4); // Motor Speed in RPM at the wheel + + if(pulses_motor2 == 0){ + mes_vel2 = 0; //Accounting for the case of having desired velocity equal to zero + } + + //Ramping + if(des_vel2 <= 50){ + ramp_rate2 = 8; //Default ramp rate + } + else{ + ramp_rate2 = des_vel2/75; //Faster Ramp Rate + } + + if(r2 > ramp_end){ + r2 = ramp_end; + } + + if(r2 < des_vel2){ + r2 = r2 + ramp_rate2; //Ramp up + + if(r2 > des_vel2){ + r2 = des_vel2; //Limit if it over shoots the desired + } + } + if(r2 > des_vel2){ + r2 = r2 - ramp_rate2; //Ramp down + + if(r2 < des_vel2){ + r2 = des_vel2; //Limit if it under shoots the desired + } + } + + if(mes_vel2 < 0){ + mes_vel2 = -mes_vel2; //Ensure Positive Feedback + } + e2 = r2 - mes_vel2; //Error, Ensure Negative Feedback + D_Term2 = (e2 - e_Prev2)/dt; //Slope + + u2 = abs((e2*Kp + I_Term2*Ki + D_Term2*Kd)); + + if(abs(u2) >= PWM_Period){ + u2 = PWM_Period;//Max Duty Cycle + } + else{ + if(des_vel2 != 0 && mes_vel2 != 0){ + I_Term2 = I_Term2 + e2*dt; //Previous error + error*time step, scaling factor 10e2 + } + } + + PWM_motor2.pulsewidth_us(abs(u2)); + e_Prev2 = e2; //Previous Error + motor2.reset(); //Reset the motor1 encoder + } + +} + +// ******** Encoder Interrupt Handler ******** +void Encoder_ISR(void) { + osSignalSet(EncoderID,0x1); // Send signal to the thread with ID, EncoderID, i.e., ExtThread. +} + +// ******** Periodic Interrupt Handler 1******** +void PeriodicInterruptISR(void){ + osSignalSet(PiControlId,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. +} + +// ******** Periodic Interrupt Handler 2******** +void PeriodicInterrupt2ISR(void){ + osSignalSet(PiControl2Id,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. +} +