Koceila Cherfouh
Differential Drive Robot Controlled by nRF24l01+ radio module.
Dependencies: mbed mbed-rtos QEI nRF24L01P-mbed
Revision 0:d5c0d6f47dd1, committed 2019-10-08
- Comitter:
- kcherfou
- Date:
- Tue Oct 08 13:36:16 2019 +0000
- Commit message:
- Differential Drive Robot controlled by nRF24l01+ radio module
Changed in this revision
diff -r 000000000000 -r d5c0d6f47dd1 main.cpp
--- /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.
+}
+
diff -r 000000000000 -r d5c0d6f47dd1 mbed-QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-QEI.lib Tue Oct 08 13:36:16 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r d5c0d6f47dd1 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Tue Oct 08 13:36:16 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
diff -r 000000000000 -r d5c0d6f47dd1 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 08 13:36:16 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r d5c0d6f47dd1 nRF24L01P-mbed.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/nRF24L01P-mbed.lib Tue Oct 08 13:36:16 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/kcherfou/code/nRF24L01P-mbed/#eefb4f0c0b19