Koceila Cherfouh / Mbed 2 deprecated DifferentialDriveRobot

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?

UserRevisionLine numberNew 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