RASCar / Mbed 2 deprecated DrivingLayerV0004

s

Dependencies:   PwmIn mbed RASCarLED buzzer millis

main.cpp@8:32133eeb7037, 2020-01-25 (annotated)

Committer:
LordScarface
Date:
Sat Jan 25 17:11:19 2020 +0000
Revision:
8:32133eeb7037
Parent:
7:986d5298b118
Child:
9:92283a284936
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 0:f59179afee57 1 #include "mbed.h"
LordScarface 8:32133eeb7037 2 #include "millis.h"
LordScarface 8:32133eeb7037 3 #include "PwmIn.h"
LordScarface 8:32133eeb7037 4
LordScarface 8:32133eeb7037 5
LordScarface 8:32133eeb7037 6 // I/O Declaration_________________________________________________
emilmont 0:f59179afee57 7
emilmont 0:f59179afee57 8 DigitalOut myled(LED_GREEN);
LordScarface 8:32133eeb7037 9 DigitalOut red(LED_RED);
Kojto 4:d6816f97e349 10 Serial pc(USBTX, USBRX);
LordScarface 8:32133eeb7037 11 //Serial pc(PTE22, PTE23);
LordScarface 8:32133eeb7037 12
LordScarface 8:32133eeb7037 13 //OUTPUTS
LordScarface 8:32133eeb7037 14
LordScarface 8:32133eeb7037 15 PwmOut leftESC(PTA4);
LordScarface 8:32133eeb7037 16
LordScarface 8:32133eeb7037 17 PwmOut rightESC(PTA5);
LordScarface 8:32133eeb7037 18
LordScarface 8:32133eeb7037 19 PwmOut Servo(PTA12);
LordScarface 8:32133eeb7037 20
LordScarface 8:32133eeb7037 21 DigitalOut LED(PTC1);
LordScarface 8:32133eeb7037 22
LordScarface 8:32133eeb7037 23
LordScarface 8:32133eeb7037 24 //INPUTS
LordScarface 8:32133eeb7037 25
LordScarface 8:32133eeb7037 26 PwmIn steeringAngle(PTD0);
LordScarface 8:32133eeb7037 27
LordScarface 8:32133eeb7037 28 PwmIn motorSpeed(PTD5);
LordScarface 8:32133eeb7037 29
LordScarface 8:32133eeb7037 30
LordScarface 8:32133eeb7037 31 InterruptIn eBrake(PTD2);
LordScarface 8:32133eeb7037 32
LordScarface 8:32133eeb7037 33 InterruptIn RPM_one(PTD4);
LordScarface 8:32133eeb7037 34
LordScarface 8:32133eeb7037 35 InterruptIn RPM_two(PTA13);
LordScarface 8:32133eeb7037 36
LordScarface 8:32133eeb7037 37 AnalogIn poti(PTB3);
LordScarface 8:32133eeb7037 38
LordScarface 8:32133eeb7037 39 AnalogIn poti2(PTB1);
LordScarface 8:32133eeb7037 40
LordScarface 8:32133eeb7037 41 DigitalIn fast(PTD1);
LordScarface 8:32133eeb7037 42
LordScarface 8:32133eeb7037 43
LordScarface 8:32133eeb7037 44
LordScarface 8:32133eeb7037 45 //Declarations for RPM measurement___________________
LordScarface 8:32133eeb7037 46 Timer RPM_timer_one;
LordScarface 8:32133eeb7037 47 long RPM_spent_time_one = 0;
LordScarface 8:32133eeb7037 48 long RPM_val_one = 0;
LordScarface 8:32133eeb7037 49
LordScarface 8:32133eeb7037 50 Timer RPM_timer_two;
LordScarface 8:32133eeb7037 51 long RPM_spent_time_two = 0;
LordScarface 8:32133eeb7037 52 long RPM_val_two = 0;
LordScarface 8:32133eeb7037 53
LordScarface 8:32133eeb7037 54 const int numReadings_one = 8;
LordScarface 8:32133eeb7037 55 int RPMs_one[numReadings_one];
LordScarface 8:32133eeb7037 56 int readIndex_one = 0;
LordScarface 8:32133eeb7037 57 int total_one = 0;
LordScarface 8:32133eeb7037 58 int averageRPM_one = 0;
LordScarface 8:32133eeb7037 59 long speed_one = 0;
LordScarface 8:32133eeb7037 60
LordScarface 8:32133eeb7037 61 const int numReadings_two = 8;
LordScarface 8:32133eeb7037 62 int RPMs_two[numReadings_two];
LordScarface 8:32133eeb7037 63 int readIndex_two = 0;
LordScarface 8:32133eeb7037 64 int total_two = 0;
LordScarface 8:32133eeb7037 65 int averageRPM_two = 0;
LordScarface 8:32133eeb7037 66 long speed_two = 0;
LordScarface 8:32133eeb7037 67 //__________________________________________________________
LordScarface 8:32133eeb7037 68
LordScarface 8:32133eeb7037 69 //dings
LordScarface 8:32133eeb7037 70 float prev_val = 0;
LordScarface 8:32133eeb7037 71
LordScarface 8:32133eeb7037 72
LordScarface 8:32133eeb7037 73 //INTERRUPT SERVICE ROUNTINES_______________________________
LordScarface 8:32133eeb7037 74 void RPM_one_ISR()
LordScarface 8:32133eeb7037 75 {
LordScarface 8:32133eeb7037 76 RPM_one.rise(NULL);
LordScarface 8:32133eeb7037 77 RPM_spent_time_one = RPM_timer_one.read_high_resolution_us();
LordScarface 8:32133eeb7037 78 RPM_timer_one.reset();
LordScarface 8:32133eeb7037 79 RPM_one.rise(&RPM_one_ISR);
LordScarface 8:32133eeb7037 80 }
LordScarface 8:32133eeb7037 81
LordScarface 8:32133eeb7037 82 void RPM_two_ISR()
LordScarface 8:32133eeb7037 83 {
LordScarface 8:32133eeb7037 84 RPM_two.rise(NULL);
LordScarface 8:32133eeb7037 85 RPM_spent_time_two = RPM_timer_two.read_high_resolution_us();
LordScarface 8:32133eeb7037 86 RPM_timer_two.reset();
LordScarface 8:32133eeb7037 87 RPM_two.rise(&RPM_two_ISR);
LordScarface 8:32133eeb7037 88 }
LordScarface 8:32133eeb7037 89
LordScarface 8:32133eeb7037 90 bool brake = false;
LordScarface 8:32133eeb7037 91 void eBrake_ISR()
LordScarface 8:32133eeb7037 92 {
LordScarface 8:32133eeb7037 93 myled = !myled;
LordScarface 8:32133eeb7037 94 brake = true;
LordScarface 8:32133eeb7037 95 }
LordScarface 8:32133eeb7037 96 void eBrake2_ISR()
LordScarface 8:32133eeb7037 97 {
LordScarface 8:32133eeb7037 98 myled = !myled;
LordScarface 8:32133eeb7037 99 brake = false;
LordScarface 8:32133eeb7037 100 }
LordScarface 8:32133eeb7037 101
LordScarface 8:32133eeb7037 102 //___________________________________________________________
LordScarface 8:32133eeb7037 103
LordScarface 8:32133eeb7037 104 //MAPPING FUNCTION
LordScarface 8:32133eeb7037 105 float map(float x, float in_min, float in_max, float out_min, float out_max)
LordScarface 8:32133eeb7037 106 {
LordScarface 8:32133eeb7037 107 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
LordScarface 8:32133eeb7037 108 }
LordScarface 8:32133eeb7037 109
LordScarface 8:32133eeb7037 110 void setRightESC(float val)
LordScarface 8:32133eeb7037 111 {
LordScarface 8:32133eeb7037 112 if (val != prev_val) {
LordScarface 8:32133eeb7037 113 rightESC.write(val);
LordScarface 8:32133eeb7037 114 prev_val = val;
LordScarface 8:32133eeb7037 115 }
LordScarface 8:32133eeb7037 116 }
LordScarface 8:32133eeb7037 117
LordScarface 8:32133eeb7037 118 volatile bool newData = false;
LordScarface 8:32133eeb7037 119 volatile float inputs[3];
LordScarface 8:32133eeb7037 120
LordScarface 8:32133eeb7037 121
LordScarface 8:32133eeb7037 122
LordScarface 8:32133eeb7037 123
LordScarface 8:32133eeb7037 124 Timer speedTimer;
LordScarface 8:32133eeb7037 125
LordScarface 8:32133eeb7037 126 //PID-Stuff-------------------------------------------------------------
LordScarface 8:32133eeb7037 127 float PID_error, PID_error_pre, PID_out, PID_sum, PID_val;
LordScarface 8:32133eeb7037 128 float leftRPM, rightRPM;
LordScarface 8:32133eeb7037 129 float set_speed = 300; //desired RPM value
LordScarface 8:32133eeb7037 130 float Kp = 1;
LordScarface 8:32133eeb7037 131 float Ki = 0;
LordScarface 8:32133eeb7037 132 float Kd = 0;
LordScarface 8:32133eeb7037 133 //----------------------------------------------------------------------
LordScarface 8:32133eeb7037 134
LordScarface 8:32133eeb7037 135 void calcPID()
LordScarface 8:32133eeb7037 136 {
LordScarface 8:32133eeb7037 137 PID_error_pre = PID_error;
LordScarface 8:32133eeb7037 138 PID_error = set_speed - leftRPM;
LordScarface 8:32133eeb7037 139
LordScarface 8:32133eeb7037 140 PID_val = Kp * PID_error + Ki * PID_sum + Kd * (PID_error - PID_error_pre);
LordScarface 8:32133eeb7037 141 PID_out += PID_val;
LordScarface 8:32133eeb7037 142
LordScarface 8:32133eeb7037 143 PID_sum += PID_error;
LordScarface 8:32133eeb7037 144
LordScarface 8:32133eeb7037 145 if (PID_sum > 500) {
LordScarface 8:32133eeb7037 146 PID_sum = 500;
LordScarface 8:32133eeb7037 147 } else if (PID_sum < -500) {
LordScarface 8:32133eeb7037 148 PID_sum = -500;
LordScarface 8:32133eeb7037 149 }
LordScarface 8:32133eeb7037 150
LordScarface 8:32133eeb7037 151 if (PID_out > 500) {
LordScarface 8:32133eeb7037 152 PID_out = 500;
LordScarface 8:32133eeb7037 153 } else if (PID_out < -500) {
LordScarface 8:32133eeb7037 154 PID_out = -500;
LordScarface 8:32133eeb7037 155 }
LordScarface 8:32133eeb7037 156 }
LordScarface 8:32133eeb7037 157
emilmont 0:f59179afee57 158
sam_grove 7:986d5298b118 159 int main()
sam_grove 7:986d5298b118 160 {
LordScarface 8:32133eeb7037 161 pc.printf("Ready!\n");
LordScarface 8:32133eeb7037 162
LordScarface 8:32133eeb7037 163 pc.baud(115200);
LordScarface 8:32133eeb7037 164
LordScarface 8:32133eeb7037 165
LordScarface 8:32133eeb7037 166 //Settings for Interrrupts for RPM Sensors
LordScarface 8:32133eeb7037 167 RPM_one.rise(&RPM_one_ISR);
LordScarface 8:32133eeb7037 168 RPM_timer_one.start();
LordScarface 8:32133eeb7037 169 RPM_two.rise(&RPM_two_ISR);
LordScarface 8:32133eeb7037 170 RPM_timer_two.start();
LordScarface 8:32133eeb7037 171
LordScarface 8:32133eeb7037 172 //Setting for EBrake Interrrupt
LordScarface 8:32133eeb7037 173 eBrake.rise(&eBrake_ISR);
LordScarface 8:32133eeb7037 174 eBrake.fall(&eBrake2_ISR);
LordScarface 8:32133eeb7037 175
LordScarface 8:32133eeb7037 176 //Values for Servo Passthrough
LordScarface 8:32133eeb7037 177 float dutycycle = 0;
LordScarface 8:32133eeb7037 178 float pulsewidth = 0;
LordScarface 8:32133eeb7037 179 float period = 0;
LordScarface 8:32133eeb7037 180
LordScarface 8:32133eeb7037 181 //Servo Settings
LordScarface 8:32133eeb7037 182 period = steeringAngle.period();
LordScarface 8:32133eeb7037 183 Servo.period_ms(20);
LordScarface 8:32133eeb7037 184 //pc.printf("%f",period
sam_grove 7:986d5298b118 185
LordScarface 8:32133eeb7037 186 //ESC Settings
LordScarface 8:32133eeb7037 187 leftESC.period_us(10000);
LordScarface 8:32133eeb7037 188 rightESC.period_us(10000);
LordScarface 8:32133eeb7037 189
LordScarface 8:32133eeb7037 190 //ARMING ESCS
LordScarface 8:32133eeb7037 191 leftESC.pulsewidth_us(1500); //write them to idle position
LordScarface 8:32133eeb7037 192 rightESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 193 wait(0.9f); //wait 2 Seconds
LordScarface 8:32133eeb7037 194 wait(0.9f);
LordScarface 8:32133eeb7037 195 wait(0.2f);
LordScarface 8:32133eeb7037 196 wait(0.9f); //wait 2 Seconds
LordScarface 8:32133eeb7037 197 wait(0.9f);
LordScarface 8:32133eeb7037 198 wait(0.2f);
LordScarface 8:32133eeb7037 199
LordScarface 8:32133eeb7037 200
LordScarface 8:32133eeb7037 201
LordScarface 8:32133eeb7037 202
LordScarface 8:32133eeb7037 203 speedTimer.start();
LordScarface 8:32133eeb7037 204 Timer ppp;
LordScarface 8:32133eeb7037 205 float poti_val = 0;
LordScarface 8:32133eeb7037 206
LordScarface 8:32133eeb7037 207 bool locked = false;
LordScarface 8:32133eeb7037 208
LordScarface 8:32133eeb7037 209
LordScarface 8:32133eeb7037 210 //MAIN LOOP_________________________________________________________________
emilmont 1:32eacc4f6beb 211 while (true) {
LordScarface 8:32133eeb7037 212
LordScarface 8:32133eeb7037 213
LordScarface 8:32133eeb7037 214 //MEASURE RPM___________________________________________________________
LordScarface 8:32133eeb7037 215 total_one = total_one - RPMs_one[readIndex_one];
LordScarface 8:32133eeb7037 216 total_two = total_two - RPMs_two[readIndex_two];
LordScarface 8:32133eeb7037 217
LordScarface 8:32133eeb7037 218 if (RPM_timer_one.read_high_resolution_us() > 500000) {
LordScarface 8:32133eeb7037 219 RPM_val_one = 0;
LordScarface 8:32133eeb7037 220 } else if (RPM_spent_time_one > 0) {
LordScarface 8:32133eeb7037 221 RPM_val_one = 60000000/(RPM_spent_time_one * 8);
LordScarface 8:32133eeb7037 222 } else {
LordScarface 8:32133eeb7037 223 RPM_val_one = 0;
LordScarface 8:32133eeb7037 224 }
LordScarface 8:32133eeb7037 225
LordScarface 8:32133eeb7037 226 if (RPM_timer_two.read_high_resolution_us() > 500000) {
LordScarface 8:32133eeb7037 227 RPM_val_two = 0;
LordScarface 8:32133eeb7037 228 } else if (RPM_spent_time_two > 0) {
LordScarface 8:32133eeb7037 229 RPM_val_two = 60000000/(RPM_spent_time_two * 8);
LordScarface 8:32133eeb7037 230 } else {
LordScarface 8:32133eeb7037 231 RPM_val_two = 0;
LordScarface 8:32133eeb7037 232 }
LordScarface 8:32133eeb7037 233
LordScarface 8:32133eeb7037 234 RPMs_one[readIndex_one] = RPM_val_one;
LordScarface 8:32133eeb7037 235 total_one = total_one + RPMs_one[readIndex_one];
LordScarface 8:32133eeb7037 236 readIndex_one++;
LordScarface 8:32133eeb7037 237
LordScarface 8:32133eeb7037 238 RPMs_two[readIndex_two] = RPM_val_two;
LordScarface 8:32133eeb7037 239 total_two = total_two + RPMs_two[readIndex_two];
LordScarface 8:32133eeb7037 240 readIndex_two++;
LordScarface 8:32133eeb7037 241
LordScarface 8:32133eeb7037 242 //set rpms to zero if under certain speed
LordScarface 8:32133eeb7037 243 if (readIndex_one >= numReadings_one) {
LordScarface 8:32133eeb7037 244 readIndex_one = 0;
LordScarface 8:32133eeb7037 245 }
LordScarface 8:32133eeb7037 246
LordScarface 8:32133eeb7037 247 if (readIndex_two >= numReadings_two) {
LordScarface 8:32133eeb7037 248 readIndex_two = 0;
LordScarface 8:32133eeb7037 249 }
LordScarface 8:32133eeb7037 250
LordScarface 8:32133eeb7037 251 //average the rpms since last run of pid error
LordScarface 8:32133eeb7037 252 averageRPM_one = total_one / numReadings_one;
LordScarface 8:32133eeb7037 253 averageRPM_two = total_two / numReadings_two;
LordScarface 8:32133eeb7037 254
LordScarface 8:32133eeb7037 255 //store pid output
LordScarface 8:32133eeb7037 256 leftRPM = averageRPM_two;
LordScarface 8:32133eeb7037 257 rightRPM = averageRPM_one;
LordScarface 8:32133eeb7037 258
LordScarface 8:32133eeb7037 259 //calculate actual pid error
LordScarface 8:32133eeb7037 260 calcPID();
LordScarface 8:32133eeb7037 261
LordScarface 8:32133eeb7037 262 //check for drivng switch to be pressed
LordScarface 8:32133eeb7037 263 if(fast && !brake) {
LordScarface 8:32133eeb7037 264 //leftESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
LordScarface 8:32133eeb7037 265 //rightESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
LordScarface 8:32133eeb7037 266 leftESC.pulsewidth_us(1500 + poti_val);
LordScarface 8:32133eeb7037 267 rightESC.pulsewidth_us(1500 + poti_val);
LordScarface 8:32133eeb7037 268 LED = 1;
LordScarface 8:32133eeb7037 269 locked = false;
LordScarface 8:32133eeb7037 270 }else if(fast && brake && !locked){
LordScarface 8:32133eeb7037 271 locked = true;
LordScarface 8:32133eeb7037 272 leftESC.pulsewidth_us(1400);
LordScarface 8:32133eeb7037 273 rightESC.pulsewidth_us(1400);
LordScarface 8:32133eeb7037 274 wait(0.5f);
LordScarface 8:32133eeb7037 275 leftESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 276 rightESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 277 }
LordScarface 8:32133eeb7037 278 else {
LordScarface 8:32133eeb7037 279 leftESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 280 rightESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 281 LED = 0;
LordScarface 8:32133eeb7037 282 }
LordScarface 8:32133eeb7037 283
LordScarface 8:32133eeb7037 284 //PASSTHROUGH FOR STEERING
LordScarface 8:32133eeb7037 285 pulsewidth = steeringAngle.pulsewidth();
LordScarface 8:32133eeb7037 286 Servo.pulsewidth_us(pulsewidth*1000000);
LordScarface 8:32133eeb7037 287
LordScarface 8:32133eeb7037 288
LordScarface 8:32133eeb7037 289 //poti_val = map( poti.read() , 0 , 1 , 0 , 1 );
LordScarface 8:32133eeb7037 290 //wait_ms(10);
LordScarface 8:32133eeb7037 291
LordScarface 8:32133eeb7037 292 poti_val = poti.read() * 500;
LordScarface 8:32133eeb7037 293 //Kp = poti_val;
LordScarface 8:32133eeb7037 294
LordScarface 8:32133eeb7037 295
LordScarface 8:32133eeb7037 296 //DEBUGGING___________________________________
LordScarface 8:32133eeb7037 297 /*
LordScarface 8:32133eeb7037 298 pc.printf("%f ", leftRPM);
LordScarface 8:32133eeb7037 299 pc.printf(" , ");
LordScarface 8:32133eeb7037 300 //pc.printf("%.5f ", 1500+PID_out );
LordScarface 8:32133eeb7037 301 //pc.printf(" , ");
LordScarface 8:32133eeb7037 302 pc.printf("%f ,", set_speed);
LordScarface 8:32133eeb7037 303 pc.printf("%f ,", poti_val);
LordScarface 8:32133eeb7037 304 pc.printf("0.0 \n" );
LordScarface 8:32133eeb7037 305 */
LordScarface 8:32133eeb7037 306 //_____________________________________________
LordScarface 8:32133eeb7037 307
emilmont 0:f59179afee57 308 }
LordScarface 8:32133eeb7037 309 //END OF MAIN LOOP__________________________________________________________
LordScarface 8:32133eeb7037 310 }