![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Differential Drive Robot Controlled by nRF24l01+ radio module.
Dependencies: mbed mbed-rtos QEI nRF24L01P-mbed
main.cpp@0:d5c0d6f47dd1, 2019-10-08 (annotated)
- Committer:
- kcherfou
- Date:
- Tue Oct 08 13:36:16 2019 +0000
- Revision:
- 0:d5c0d6f47dd1
Differential Drive Robot controlled by nRF24l01+ radio module
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kcherfou | 0:d5c0d6f47dd1 | 1 | #include "mbed.h" |
kcherfou | 0:d5c0d6f47dd1 | 2 | #include "nRF24L01P.h" |
kcherfou | 0:d5c0d6f47dd1 | 3 | #include "rtos.h" |
kcherfou | 0:d5c0d6f47dd1 | 4 | #include "Serial.h" |
kcherfou | 0:d5c0d6f47dd1 | 5 | #include "QEI.h" |
kcherfou | 0:d5c0d6f47dd1 | 6 | |
kcherfou | 0:d5c0d6f47dd1 | 7 | //Function Prototypes |
kcherfou | 0:d5c0d6f47dd1 | 8 | void Encoder_ISR(void); //Handler for Encoder |
kcherfou | 0:d5c0d6f47dd1 | 9 | void EncoderThread(void const *argument); //Handler for Encoder |
kcherfou | 0:d5c0d6f47dd1 | 10 | void PID1(void const *argument); //PID Control for motor 1 |
kcherfou | 0:d5c0d6f47dd1 | 11 | void PID2(void const *argument); //PID Control for motor 2 |
kcherfou | 0:d5c0d6f47dd1 | 12 | void PeriodicInterruptISR(void); //Periodic Timer Interrupt Serive Routine for Motor 1 |
kcherfou | 0:d5c0d6f47dd1 | 13 | void PeriodicInterrupt2ISR(void); //Periodic Timer Interrupt Serive Routine for Motor 2 |
kcherfou | 0:d5c0d6f47dd1 | 14 | //void AssignData(); |
kcherfou | 0:d5c0d6f47dd1 | 15 | |
kcherfou | 0:d5c0d6f47dd1 | 16 | //Processes and Threads |
kcherfou | 0:d5c0d6f47dd1 | 17 | osThreadId EncoderID, Encoder2ID, PiControlId, PiControl2Id; // Thread ID's |
kcherfou | 0:d5c0d6f47dd1 | 18 | osThreadDef(PID1, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority |
kcherfou | 0:d5c0d6f47dd1 | 19 | osThreadDef(PID2, osPriorityRealtime, 1024); // Declare PiControlThread2 as a thread, highest priority |
kcherfou | 0:d5c0d6f47dd1 | 20 | osThreadDef(EncoderThread, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority |
kcherfou | 0:d5c0d6f47dd1 | 21 | |
kcherfou | 0:d5c0d6f47dd1 | 22 | Serial pc(USBTX, USBRX); // tx, rx |
kcherfou | 0:d5c0d6f47dd1 | 23 | nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTD0, PTE0, PTD5); // mosi, miso, sck, csn, ce, irq |
kcherfou | 0:d5c0d6f47dd1 | 24 | DigitalOut myled1(LED1); |
kcherfou | 0:d5c0d6f47dd1 | 25 | DigitalOut myled2(LED2); |
kcherfou | 0:d5c0d6f47dd1 | 26 | |
kcherfou | 0:d5c0d6f47dd1 | 27 | // Data Packets Requirements |
kcherfou | 0:d5c0d6f47dd1 | 28 | #define StartBit 0xFF |
kcherfou | 0:d5c0d6f47dd1 | 29 | #define IDbit 0x01 |
kcherfou | 0:d5c0d6f47dd1 | 30 | #define EndBit 0xFF |
kcherfou | 0:d5c0d6f47dd1 | 31 | |
kcherfou | 0:d5c0d6f47dd1 | 32 | #define TRANSFER_SIZE 8 |
kcherfou | 0:d5c0d6f47dd1 | 33 | |
kcherfou | 0:d5c0d6f47dd1 | 34 | char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE]; |
kcherfou | 0:d5c0d6f47dd1 | 35 | int txDataCnt = 0; |
kcherfou | 0:d5c0d6f47dd1 | 36 | int rxDataCnt = 0; |
kcherfou | 0:d5c0d6f47dd1 | 37 | |
kcherfou | 0:d5c0d6f47dd1 | 38 | |
kcherfou | 0:d5c0d6f47dd1 | 39 | //Port Configuration |
kcherfou | 0:d5c0d6f47dd1 | 40 | //Motor Directions |
kcherfou | 0:d5c0d6f47dd1 | 41 | DigitalOut dir_motor1(PTE4); //Direction Motor 1 |
kcherfou | 0:d5c0d6f47dd1 | 42 | DigitalOut dir_motor2(PTE5); //Direction Motor 2 |
kcherfou | 0:d5c0d6f47dd1 | 43 | |
kcherfou | 0:d5c0d6f47dd1 | 44 | //PWM Outputs |
kcherfou | 0:d5c0d6f47dd1 | 45 | PwmOut PWM_motor1(PTE20); //PWM Motor2 |
kcherfou | 0:d5c0d6f47dd1 | 46 | PwmOut PWM_motor2(PTE22); //PWM Motor1 |
kcherfou | 0:d5c0d6f47dd1 | 47 | |
kcherfou | 0:d5c0d6f47dd1 | 48 | //Kick Output |
kcherfou | 0:d5c0d6f47dd1 | 49 | DigitalOut kick(PTB10); //Kick command |
kcherfou | 0:d5c0d6f47dd1 | 50 | //Encoder |
kcherfou | 0:d5c0d6f47dd1 | 51 | QEI motor1(PTD4, PTA12, NC, 16, QEI::X4_ENCODING); //QEI for motor 1 |
kcherfou | 0:d5c0d6f47dd1 | 52 | QEI motor2(PTA4, PTA5, NC, 16, QEI::X4_ENCODING); //QEI for motor 2 |
kcherfou | 0:d5c0d6f47dd1 | 53 | //Timer Interrupts |
kcherfou | 0:d5c0d6f47dd1 | 54 | Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt |
kcherfou | 0:d5c0d6f47dd1 | 55 | Ticker PeriodicInt2; // Declare a timer interrupt: PeriodicInt2 |
kcherfou | 0:d5c0d6f47dd1 | 56 | Ticker Encoder_Tick; //Timer Interrupt for counting Pulses |
kcherfou | 0:d5c0d6f47dd1 | 57 | |
kcherfou | 0:d5c0d6f47dd1 | 58 | |
kcherfou | 0:d5c0d6f47dd1 | 59 | |
kcherfou | 0:d5c0d6f47dd1 | 60 | |
kcherfou | 0:d5c0d6f47dd1 | 61 | //Declare Global Variables |
kcherfou | 0:d5c0d6f47dd1 | 62 | float des_vel = 0, mes_vel = 0, des_vel2 = 0, mes_vel2 = 0; |
kcherfou | 0:d5c0d6f47dd1 | 63 | float e = 0, e_Prev = 0, e2 = 0, e_Prev2 = 0; |
kcherfou | 0:d5c0d6f47dd1 | 64 | float I_Term = 0, I_Term2 = 0, D_Term = 0, D_Term2 = 0; |
kcherfou | 0:d5c0d6f47dd1 | 65 | 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 |
kcherfou | 0:d5c0d6f47dd1 | 66 | float u = 0, u2 = 0; |
kcherfou | 0:d5c0d6f47dd1 | 67 | float r = 50, r2 = 50; |
kcherfou | 0:d5c0d6f47dd1 | 68 | 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? |
kcherfou | 0:d5c0d6f47dd1 | 69 | float dt = 0.05; //dt best = 0.05? |
kcherfou | 0:d5c0d6f47dd1 | 70 | float pulses_motor1, pulses_motor2; |
kcherfou | 0:d5c0d6f47dd1 | 71 | float PWM_Period = 2000; //in micro-seconds |
kcherfou | 0:d5c0d6f47dd1 | 72 | |
kcherfou | 0:d5c0d6f47dd1 | 73 | |
kcherfou | 0:d5c0d6f47dd1 | 74 | //Testing Digital Input for LED |
kcherfou | 0:d5c0d6f47dd1 | 75 | //DigitalOut led3(LED3); |
kcherfou | 0:d5c0d6f47dd1 | 76 | |
kcherfou | 0:d5c0d6f47dd1 | 77 | |
kcherfou | 0:d5c0d6f47dd1 | 78 | int main() { |
kcherfou | 0:d5c0d6f47dd1 | 79 | |
kcherfou | 0:d5c0d6f47dd1 | 80 | // The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's |
kcherfou | 0:d5c0d6f47dd1 | 81 | // "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019) |
kcherfou | 0:d5c0d6f47dd1 | 82 | // only handles 4 byte transfers in the ATMega code. |
kcherfou | 0:d5c0d6f47dd1 | 83 | pc.baud(9600); // Set max uart baud rate |
kcherfou | 0:d5c0d6f47dd1 | 84 | pc.printf("We have good serial communication! :D\n"); |
kcherfou | 0:d5c0d6f47dd1 | 85 | |
kcherfou | 0:d5c0d6f47dd1 | 86 | //Initializing Threads for interrupts |
kcherfou | 0:d5c0d6f47dd1 | 87 | PiControlId = osThreadCreate(osThread(PID1), NULL); //Create Thread for PID1 |
kcherfou | 0:d5c0d6f47dd1 | 88 | PiControl2Id = osThreadCreate(osThread(PID2), NULL); //Create Thread for PID2 |
kcherfou | 0:d5c0d6f47dd1 | 89 | EncoderID = osThreadCreate(osThread(EncoderThread), NULL); //Create Encoder Thread |
kcherfou | 0:d5c0d6f47dd1 | 90 | |
kcherfou | 0:d5c0d6f47dd1 | 91 | //PWM Period |
kcherfou | 0:d5c0d6f47dd1 | 92 | PWM_motor1.period_us(PWM_Period); // This sets the PWM period to 2000 us = 500Hz |
kcherfou | 0:d5c0d6f47dd1 | 93 | PWM_motor2.period_us(PWM_Period); // This sets the PWM period to 2000 us = 500Hz |
kcherfou | 0:d5c0d6f47dd1 | 94 | |
kcherfou | 0:d5c0d6f47dd1 | 95 | //Periodic Interrupts |
kcherfou | 0:d5c0d6f47dd1 | 96 | PeriodicInt.attach(&PeriodicInterruptISR, dt); //Periodic timer interrupt every 0.02 seconds |
kcherfou | 0:d5c0d6f47dd1 | 97 | PeriodicInt2.attach(&PeriodicInterrupt2ISR, dt); //Periodic timer interrupt every 0.02 seconds |
kcherfou | 0:d5c0d6f47dd1 | 98 | Encoder_Tick.attach(&Encoder_ISR, 4*dt); |
kcherfou | 0:d5c0d6f47dd1 | 99 | |
kcherfou | 0:d5c0d6f47dd1 | 100 | my_nrf24l01p.powerUp(); |
kcherfou | 0:d5c0d6f47dd1 | 101 | my_nrf24l01p.setAirDataRate(NRF24L01P_DATARATE_2_MBPS); |
kcherfou | 0:d5c0d6f47dd1 | 102 | |
kcherfou | 0:d5c0d6f47dd1 | 103 | // Display the (default) setup of the nRF24L01+ chip |
kcherfou | 0:d5c0d6f47dd1 | 104 | pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() ); |
kcherfou | 0:d5c0d6f47dd1 | 105 | pc.printf( "nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() ); |
kcherfou | 0:d5c0d6f47dd1 | 106 | pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() ); |
kcherfou | 0:d5c0d6f47dd1 | 107 | pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() ); |
kcherfou | 0:d5c0d6f47dd1 | 108 | pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() ); |
kcherfou | 0:d5c0d6f47dd1 | 109 | |
kcherfou | 0:d5c0d6f47dd1 | 110 | pc.printf( "Type keys to test transfers:\r\n (transfers are grouped into %d characters)\r\n", TRANSFER_SIZE ); |
kcherfou | 0:d5c0d6f47dd1 | 111 | |
kcherfou | 0:d5c0d6f47dd1 | 112 | my_nrf24l01p.setTransferSize( TRANSFER_SIZE ); |
kcherfou | 0:d5c0d6f47dd1 | 113 | |
kcherfou | 0:d5c0d6f47dd1 | 114 | my_nrf24l01p.setReceiveMode(); |
kcherfou | 0:d5c0d6f47dd1 | 115 | my_nrf24l01p.enable(); |
kcherfou | 0:d5c0d6f47dd1 | 116 | |
kcherfou | 0:d5c0d6f47dd1 | 117 | |
kcherfou | 0:d5c0d6f47dd1 | 118 | kick=0; |
kcherfou | 0:d5c0d6f47dd1 | 119 | |
kcherfou | 0:d5c0d6f47dd1 | 120 | char test = 0; |
kcherfou | 0:d5c0d6f47dd1 | 121 | //Loop Forever |
kcherfou | 0:d5c0d6f47dd1 | 122 | while (1) { |
kcherfou | 0:d5c0d6f47dd1 | 123 | /* |
kcherfou | 0:d5c0d6f47dd1 | 124 | // If we've received anything over the host serial link... |
kcherfou | 0:d5c0d6f47dd1 | 125 | if ( pc.readable() ) { |
kcherfou | 0:d5c0d6f47dd1 | 126 | |
kcherfou | 0:d5c0d6f47dd1 | 127 | // ...add it to the transmit buffer |
kcherfou | 0:d5c0d6f47dd1 | 128 | txData[txDataCnt++] = pc.getc(); |
kcherfou | 0:d5c0d6f47dd1 | 129 | |
kcherfou | 0:d5c0d6f47dd1 | 130 | // If the transmit buffer is full |
kcherfou | 0:d5c0d6f47dd1 | 131 | if ( txDataCnt >= sizeof( txData ) ) { |
kcherfou | 0:d5c0d6f47dd1 | 132 | |
kcherfou | 0:d5c0d6f47dd1 | 133 | // Send the transmitbuffer via the nRF24L01+ |
kcherfou | 0:d5c0d6f47dd1 | 134 | my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt ); |
kcherfou | 0:d5c0d6f47dd1 | 135 | |
kcherfou | 0:d5c0d6f47dd1 | 136 | txDataCnt = 0; |
kcherfou | 0:d5c0d6f47dd1 | 137 | } |
kcherfou | 0:d5c0d6f47dd1 | 138 | |
kcherfou | 0:d5c0d6f47dd1 | 139 | // Toggle LED1 (to help debug Host -> nRF24L01+ communication) |
kcherfou | 0:d5c0d6f47dd1 | 140 | myled1 = !myled1; |
kcherfou | 0:d5c0d6f47dd1 | 141 | }*/ |
kcherfou | 0:d5c0d6f47dd1 | 142 | |
kcherfou | 0:d5c0d6f47dd1 | 143 | // If we've received anything in the nRF24L01+... |
kcherfou | 0:d5c0d6f47dd1 | 144 | if ( my_nrf24l01p.readable() ) { |
kcherfou | 0:d5c0d6f47dd1 | 145 | |
kcherfou | 0:d5c0d6f47dd1 | 146 | // ...read the data into the receive buffer |
kcherfou | 0:d5c0d6f47dd1 | 147 | rxDataCnt = my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) ); |
kcherfou | 0:d5c0d6f47dd1 | 148 | if((rxData[0] == StartBit) && (rxData[1] == IDbit) && (rxData[7] == EndBit)){ |
kcherfou | 0:d5c0d6f47dd1 | 149 | // Transmitted Data |
kcherfou | 0:d5c0d6f47dd1 | 150 | des_vel = rxData[2]; |
kcherfou | 0:d5c0d6f47dd1 | 151 | dir_motor1=rxData[3]; |
kcherfou | 0:d5c0d6f47dd1 | 152 | des_vel2 = rxData[4]; |
kcherfou | 0:d5c0d6f47dd1 | 153 | dir_motor2=rxData[5]; |
kcherfou | 0:d5c0d6f47dd1 | 154 | kick = rxData[6]; |
kcherfou | 0:d5c0d6f47dd1 | 155 | // led3 = !led3; |
kcherfou | 0:d5c0d6f47dd1 | 156 | } |
kcherfou | 0:d5c0d6f47dd1 | 157 | |
kcherfou | 0:d5c0d6f47dd1 | 158 | // AssignData(); |
kcherfou | 0:d5c0d6f47dd1 | 159 | // Display the receive buffer contents via the host serial link |
kcherfou | 0:d5c0d6f47dd1 | 160 | /*for ( int i = 0; rxDataCnt > 0; rxDataCnt--, i++ ) { |
kcherfou | 0:d5c0d6f47dd1 | 161 | |
kcherfou | 0:d5c0d6f47dd1 | 162 | pc.putc( rxData[i] ); |
kcherfou | 0:d5c0d6f47dd1 | 163 | }*/ |
kcherfou | 0:d5c0d6f47dd1 | 164 | pc.putc('\n)'); |
kcherfou | 0:d5c0d6f47dd1 | 165 | pc.putc('\r'); |
kcherfou | 0:d5c0d6f47dd1 | 166 | pc.putc(rxData[0]); |
kcherfou | 0:d5c0d6f47dd1 | 167 | pc.putc(rxData[1]); |
kcherfou | 0:d5c0d6f47dd1 | 168 | pc.putc(rxData[2]); |
kcherfou | 0:d5c0d6f47dd1 | 169 | pc.putc(rxData[3]); |
kcherfou | 0:d5c0d6f47dd1 | 170 | pc.putc(rxData[4]); |
kcherfou | 0:d5c0d6f47dd1 | 171 | pc.putc(rxData[5]); |
kcherfou | 0:d5c0d6f47dd1 | 172 | pc.putc(rxData[6]); |
kcherfou | 0:d5c0d6f47dd1 | 173 | pc.putc(rxData[7]); |
kcherfou | 0:d5c0d6f47dd1 | 174 | pc.putc('\n'); |
kcherfou | 0:d5c0d6f47dd1 | 175 | pc.putc('\r'); |
kcherfou | 0:d5c0d6f47dd1 | 176 | |
kcherfou | 0:d5c0d6f47dd1 | 177 | // Toggle LED2 (to help debug nRF24L01+ -> Host communication) |
kcherfou | 0:d5c0d6f47dd1 | 178 | myled2 = !myled2; |
kcherfou | 0:d5c0d6f47dd1 | 179 | } |
kcherfou | 0:d5c0d6f47dd1 | 180 | } |
kcherfou | 0:d5c0d6f47dd1 | 181 | } |
kcherfou | 0:d5c0d6f47dd1 | 182 | |
kcherfou | 0:d5c0d6f47dd1 | 183 | |
kcherfou | 0:d5c0d6f47dd1 | 184 | |
kcherfou | 0:d5c0d6f47dd1 | 185 | /* |
kcherfou | 0:d5c0d6f47dd1 | 186 | void AssignData(){ |
kcherfou | 0:d5c0d6f47dd1 | 187 | if((rxData[0] == StartBit) && (rxData[1] == IDbit) && (rxData[7] == EndBit)){ |
kcherfou | 0:d5c0d6f47dd1 | 188 | // Transmitted Data |
kcherfou | 0:d5c0d6f47dd1 | 189 | des_vel = rxData[2]; |
kcherfou | 0:d5c0d6f47dd1 | 190 | dir_motor1=rxData[3]; |
kcherfou | 0:d5c0d6f47dd1 | 191 | des_vel2 = rxData[4]; |
kcherfou | 0:d5c0d6f47dd1 | 192 | dir_motor2=rxData[5]; |
kcherfou | 0:d5c0d6f47dd1 | 193 | kick = rxData[6]; |
kcherfou | 0:d5c0d6f47dd1 | 194 | // led3 = !led3; |
kcherfou | 0:d5c0d6f47dd1 | 195 | } |
kcherfou | 0:d5c0d6f47dd1 | 196 | } |
kcherfou | 0:d5c0d6f47dd1 | 197 | |
kcherfou | 0:d5c0d6f47dd1 | 198 | */ |
kcherfou | 0:d5c0d6f47dd1 | 199 | |
kcherfou | 0:d5c0d6f47dd1 | 200 | // ******** Encoder Thread ******** |
kcherfou | 0:d5c0d6f47dd1 | 201 | void EncoderThread(void const *argument){ |
kcherfou | 0:d5c0d6f47dd1 | 202 | while(true){ |
kcherfou | 0:d5c0d6f47dd1 | 203 | osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received |
kcherfou | 0:d5c0d6f47dd1 | 204 | |
kcherfou | 0:d5c0d6f47dd1 | 205 | pulses_motor1 = abs(motor1.getPulses()); //Get number of pulses from motor1 |
kcherfou | 0:d5c0d6f47dd1 | 206 | pulses_motor2 = abs(motor2.getPulses()); //Get number of pulses from motor2 |
kcherfou | 0:d5c0d6f47dd1 | 207 | } |
kcherfou | 0:d5c0d6f47dd1 | 208 | } |
kcherfou | 0:d5c0d6f47dd1 | 209 | |
kcherfou | 0:d5c0d6f47dd1 | 210 | // ******** PID1 Thread ******** |
kcherfou | 0:d5c0d6f47dd1 | 211 | void PID1(void const *argument){ |
kcherfou | 0:d5c0d6f47dd1 | 212 | |
kcherfou | 0:d5c0d6f47dd1 | 213 | while(true){ |
kcherfou | 0:d5c0d6f47dd1 | 214 | osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. |
kcherfou | 0:d5c0d6f47dd1 | 215 | //dir_motor1 = 1; |
kcherfou | 0:d5c0d6f47dd1 | 216 | //pulses_motor1 = abs(motor1.getPulses()); //Get number of pulses from motor1 |
kcherfou | 0:d5c0d6f47dd1 | 217 | //des_vel = 30; |
kcherfou | 0:d5c0d6f47dd1 | 218 | mes_vel = (60*pulses_motor1) / (700 * dt * 4); //Calculate the measured speed |
kcherfou | 0:d5c0d6f47dd1 | 219 | |
kcherfou | 0:d5c0d6f47dd1 | 220 | if(pulses_motor1 == 0){ |
kcherfou | 0:d5c0d6f47dd1 | 221 | mes_vel = 0; //Accounting for the case of having desired velocity equal to zero |
kcherfou | 0:d5c0d6f47dd1 | 222 | } |
kcherfou | 0:d5c0d6f47dd1 | 223 | |
kcherfou | 0:d5c0d6f47dd1 | 224 | if(r > ramp_end){ |
kcherfou | 0:d5c0d6f47dd1 | 225 | r = ramp_end; |
kcherfou | 0:d5c0d6f47dd1 | 226 | } |
kcherfou | 0:d5c0d6f47dd1 | 227 | |
kcherfou | 0:d5c0d6f47dd1 | 228 | //Ramping |
kcherfou | 0:d5c0d6f47dd1 | 229 | if(des_vel <= 50){ |
kcherfou | 0:d5c0d6f47dd1 | 230 | ramp_rate = 8; //Default ramp rate |
kcherfou | 0:d5c0d6f47dd1 | 231 | } |
kcherfou | 0:d5c0d6f47dd1 | 232 | else{ |
kcherfou | 0:d5c0d6f47dd1 | 233 | ramp_rate = des_vel/75; //Faster Ramp Rate |
kcherfou | 0:d5c0d6f47dd1 | 234 | } |
kcherfou | 0:d5c0d6f47dd1 | 235 | |
kcherfou | 0:d5c0d6f47dd1 | 236 | if(r < des_vel){ |
kcherfou | 0:d5c0d6f47dd1 | 237 | r = r + ramp_rate; //Ramp up |
kcherfou | 0:d5c0d6f47dd1 | 238 | |
kcherfou | 0:d5c0d6f47dd1 | 239 | if(r > des_vel){ |
kcherfou | 0:d5c0d6f47dd1 | 240 | r = des_vel; //Limit if it over shoots the desired |
kcherfou | 0:d5c0d6f47dd1 | 241 | } |
kcherfou | 0:d5c0d6f47dd1 | 242 | } |
kcherfou | 0:d5c0d6f47dd1 | 243 | if(r > des_vel){ |
kcherfou | 0:d5c0d6f47dd1 | 244 | r = r - ramp_rate; //Ramp down |
kcherfou | 0:d5c0d6f47dd1 | 245 | |
kcherfou | 0:d5c0d6f47dd1 | 246 | if(r < des_vel){ |
kcherfou | 0:d5c0d6f47dd1 | 247 | r = des_vel; //Limit if it under shoots the desired |
kcherfou | 0:d5c0d6f47dd1 | 248 | } |
kcherfou | 0:d5c0d6f47dd1 | 249 | } |
kcherfou | 0:d5c0d6f47dd1 | 250 | |
kcherfou | 0:d5c0d6f47dd1 | 251 | if(mes_vel < 0){ |
kcherfou | 0:d5c0d6f47dd1 | 252 | mes_vel = -mes_vel; //Ensure Positive Feedback |
kcherfou | 0:d5c0d6f47dd1 | 253 | } |
kcherfou | 0:d5c0d6f47dd1 | 254 | e = r - mes_vel; //Error |
kcherfou | 0:d5c0d6f47dd1 | 255 | D_Term = (e - e_Prev) / (dt); //Slope |
kcherfou | 0:d5c0d6f47dd1 | 256 | u = abs(e*Kp + I_Term*Ki + D_Term*Kd); |
kcherfou | 0:d5c0d6f47dd1 | 257 | |
kcherfou | 0:d5c0d6f47dd1 | 258 | if(abs(u) >= PWM_Period){ |
kcherfou | 0:d5c0d6f47dd1 | 259 | u = PWM_Period;//Max Duty Cycle |
kcherfou | 0:d5c0d6f47dd1 | 260 | } |
kcherfou | 0:d5c0d6f47dd1 | 261 | else{ |
kcherfou | 0:d5c0d6f47dd1 | 262 | if(des_vel != 0 && mes_vel != 0){ |
kcherfou | 0:d5c0d6f47dd1 | 263 | I_Term = I_Term + e*dt; //Previous error + error*time step, scaling factor 10e2 |
kcherfou | 0:d5c0d6f47dd1 | 264 | } |
kcherfou | 0:d5c0d6f47dd1 | 265 | } |
kcherfou | 0:d5c0d6f47dd1 | 266 | |
kcherfou | 0:d5c0d6f47dd1 | 267 | PWM_motor1.pulsewidth_us(abs(u)); //Set the Pulsewidth output |
kcherfou | 0:d5c0d6f47dd1 | 268 | e_Prev = e; //Previous Error |
kcherfou | 0:d5c0d6f47dd1 | 269 | motor1.reset(); //Reset the motor1 encoder |
kcherfou | 0:d5c0d6f47dd1 | 270 | } |
kcherfou | 0:d5c0d6f47dd1 | 271 | |
kcherfou | 0:d5c0d6f47dd1 | 272 | } |
kcherfou | 0:d5c0d6f47dd1 | 273 | |
kcherfou | 0:d5c0d6f47dd1 | 274 | // ******** PID2 Thread ******** |
kcherfou | 0:d5c0d6f47dd1 | 275 | void PID2(void const *argument){ |
kcherfou | 0:d5c0d6f47dd1 | 276 | |
kcherfou | 0:d5c0d6f47dd1 | 277 | while(true){ |
kcherfou | 0:d5c0d6f47dd1 | 278 | osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. |
kcherfou | 0:d5c0d6f47dd1 | 279 | |
kcherfou | 0:d5c0d6f47dd1 | 280 | //dir_motor2 = 1; |
kcherfou | 0:d5c0d6f47dd1 | 281 | //pulses_motor2 = abs(motor2.getPulses()); //Get number of pulses from motor2 |
kcherfou | 0:d5c0d6f47dd1 | 282 | //des_vel2 = 20; |
kcherfou | 0:d5c0d6f47dd1 | 283 | mes_vel2 = (60*pulses_motor2) / (700 * dt * 4); // Motor Speed in RPM at the wheel |
kcherfou | 0:d5c0d6f47dd1 | 284 | |
kcherfou | 0:d5c0d6f47dd1 | 285 | if(pulses_motor2 == 0){ |
kcherfou | 0:d5c0d6f47dd1 | 286 | mes_vel2 = 0; //Accounting for the case of having desired velocity equal to zero |
kcherfou | 0:d5c0d6f47dd1 | 287 | } |
kcherfou | 0:d5c0d6f47dd1 | 288 | |
kcherfou | 0:d5c0d6f47dd1 | 289 | //Ramping |
kcherfou | 0:d5c0d6f47dd1 | 290 | if(des_vel2 <= 50){ |
kcherfou | 0:d5c0d6f47dd1 | 291 | ramp_rate2 = 8; //Default ramp rate |
kcherfou | 0:d5c0d6f47dd1 | 292 | } |
kcherfou | 0:d5c0d6f47dd1 | 293 | else{ |
kcherfou | 0:d5c0d6f47dd1 | 294 | ramp_rate2 = des_vel2/75; //Faster Ramp Rate |
kcherfou | 0:d5c0d6f47dd1 | 295 | } |
kcherfou | 0:d5c0d6f47dd1 | 296 | |
kcherfou | 0:d5c0d6f47dd1 | 297 | if(r2 > ramp_end){ |
kcherfou | 0:d5c0d6f47dd1 | 298 | r2 = ramp_end; |
kcherfou | 0:d5c0d6f47dd1 | 299 | } |
kcherfou | 0:d5c0d6f47dd1 | 300 | |
kcherfou | 0:d5c0d6f47dd1 | 301 | if(r2 < des_vel2){ |
kcherfou | 0:d5c0d6f47dd1 | 302 | r2 = r2 + ramp_rate2; //Ramp up |
kcherfou | 0:d5c0d6f47dd1 | 303 | |
kcherfou | 0:d5c0d6f47dd1 | 304 | if(r2 > des_vel2){ |
kcherfou | 0:d5c0d6f47dd1 | 305 | r2 = des_vel2; //Limit if it over shoots the desired |
kcherfou | 0:d5c0d6f47dd1 | 306 | } |
kcherfou | 0:d5c0d6f47dd1 | 307 | } |
kcherfou | 0:d5c0d6f47dd1 | 308 | if(r2 > des_vel2){ |
kcherfou | 0:d5c0d6f47dd1 | 309 | r2 = r2 - ramp_rate2; //Ramp down |
kcherfou | 0:d5c0d6f47dd1 | 310 | |
kcherfou | 0:d5c0d6f47dd1 | 311 | if(r2 < des_vel2){ |
kcherfou | 0:d5c0d6f47dd1 | 312 | r2 = des_vel2; //Limit if it under shoots the desired |
kcherfou | 0:d5c0d6f47dd1 | 313 | } |
kcherfou | 0:d5c0d6f47dd1 | 314 | } |
kcherfou | 0:d5c0d6f47dd1 | 315 | |
kcherfou | 0:d5c0d6f47dd1 | 316 | if(mes_vel2 < 0){ |
kcherfou | 0:d5c0d6f47dd1 | 317 | mes_vel2 = -mes_vel2; //Ensure Positive Feedback |
kcherfou | 0:d5c0d6f47dd1 | 318 | } |
kcherfou | 0:d5c0d6f47dd1 | 319 | e2 = r2 - mes_vel2; //Error, Ensure Negative Feedback |
kcherfou | 0:d5c0d6f47dd1 | 320 | D_Term2 = (e2 - e_Prev2)/dt; //Slope |
kcherfou | 0:d5c0d6f47dd1 | 321 | |
kcherfou | 0:d5c0d6f47dd1 | 322 | u2 = abs((e2*Kp + I_Term2*Ki + D_Term2*Kd)); |
kcherfou | 0:d5c0d6f47dd1 | 323 | |
kcherfou | 0:d5c0d6f47dd1 | 324 | if(abs(u2) >= PWM_Period){ |
kcherfou | 0:d5c0d6f47dd1 | 325 | u2 = PWM_Period;//Max Duty Cycle |
kcherfou | 0:d5c0d6f47dd1 | 326 | } |
kcherfou | 0:d5c0d6f47dd1 | 327 | else{ |
kcherfou | 0:d5c0d6f47dd1 | 328 | if(des_vel2 != 0 && mes_vel2 != 0){ |
kcherfou | 0:d5c0d6f47dd1 | 329 | I_Term2 = I_Term2 + e2*dt; //Previous error + error*time step, scaling factor 10e2 |
kcherfou | 0:d5c0d6f47dd1 | 330 | } |
kcherfou | 0:d5c0d6f47dd1 | 331 | } |
kcherfou | 0:d5c0d6f47dd1 | 332 | |
kcherfou | 0:d5c0d6f47dd1 | 333 | PWM_motor2.pulsewidth_us(abs(u2)); |
kcherfou | 0:d5c0d6f47dd1 | 334 | e_Prev2 = e2; //Previous Error |
kcherfou | 0:d5c0d6f47dd1 | 335 | motor2.reset(); //Reset the motor1 encoder |
kcherfou | 0:d5c0d6f47dd1 | 336 | } |
kcherfou | 0:d5c0d6f47dd1 | 337 | |
kcherfou | 0:d5c0d6f47dd1 | 338 | } |
kcherfou | 0:d5c0d6f47dd1 | 339 | |
kcherfou | 0:d5c0d6f47dd1 | 340 | // ******** Encoder Interrupt Handler ******** |
kcherfou | 0:d5c0d6f47dd1 | 341 | void Encoder_ISR(void) { |
kcherfou | 0:d5c0d6f47dd1 | 342 | osSignalSet(EncoderID,0x1); // Send signal to the thread with ID, EncoderID, i.e., ExtThread. |
kcherfou | 0:d5c0d6f47dd1 | 343 | } |
kcherfou | 0:d5c0d6f47dd1 | 344 | |
kcherfou | 0:d5c0d6f47dd1 | 345 | // ******** Periodic Interrupt Handler 1******** |
kcherfou | 0:d5c0d6f47dd1 | 346 | void PeriodicInterruptISR(void){ |
kcherfou | 0:d5c0d6f47dd1 | 347 | osSignalSet(PiControlId,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. |
kcherfou | 0:d5c0d6f47dd1 | 348 | } |
kcherfou | 0:d5c0d6f47dd1 | 349 | |
kcherfou | 0:d5c0d6f47dd1 | 350 | // ******** Periodic Interrupt Handler 2******** |
kcherfou | 0:d5c0d6f47dd1 | 351 | void PeriodicInterrupt2ISR(void){ |
kcherfou | 0:d5c0d6f47dd1 | 352 | osSignalSet(PiControl2Id,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. |
kcherfou | 0:d5c0d6f47dd1 | 353 | } |
kcherfou | 0:d5c0d6f47dd1 | 354 |