RASCar / Mbed 2 deprecated DrivingLayerV0004

s

Dependencies:   PwmIn mbed RASCarLED buzzer millis

main.cpp@10:53d54dceea50, 2020-05-04 (annotated)

Committer:
LordScarface
Date:
Mon May 04 23:05:44 2020 +0000
Revision:
10:53d54dceea50
Parent:
9:92283a284936 
studio

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 9:92283a284936 4 #include "RASCarLED.h"
LordScarface 9:92283a284936 5 #include "buzzer.h"
LordScarface 8:32133eeb7037 6
LordScarface 8:32133eeb7037 7
LordScarface 8:32133eeb7037 8 // I/O Declaration_________________________________________________
emilmont 0:f59179afee57 9
LordScarface 9:92283a284936 10 //LEDs
LordScarface 9:92283a284936 11 RASCarLED leftLed(PTC7, PTC0, PTC3);
LordScarface 9:92283a284936 12 RASCarLED rightLed(PTC4, PTC5, PTC6);
LordScarface 9:92283a284936 13
LordScarface 9:92283a284936 14 DigitalOut green(LED_GREEN);
LordScarface 8:32133eeb7037 15 DigitalOut red(LED_RED);
LordScarface 9:92283a284936 16 DigitalOut blue(LED_BLUE);
LordScarface 9:92283a284936 17
LordScarface 9:92283a284936 18 //Debugging out comment out for either USB or Bluetooth
LordScarface 9:92283a284936 19 //Serial pc(USBTX, USBRX);
LordScarface 9:92283a284936 20 Serial pc(PTE22, PTE23);
LordScarface 8:32133eeb7037 21
LordScarface 8:32133eeb7037 22 //OUTPUTS
LordScarface 8:32133eeb7037 23
LordScarface 9:92283a284936 24 Beep buzzer(PTA2);
LordScarface 9:92283a284936 25
LordScarface 8:32133eeb7037 26 PwmOut leftESC(PTA4);
LordScarface 8:32133eeb7037 27
LordScarface 8:32133eeb7037 28 PwmOut rightESC(PTA5);
LordScarface 8:32133eeb7037 29
LordScarface 8:32133eeb7037 30 PwmOut Servo(PTA12);
LordScarface 8:32133eeb7037 31
LordScarface 8:32133eeb7037 32 DigitalOut LED(PTC1);
LordScarface 8:32133eeb7037 33
LordScarface 9:92283a284936 34 PwmOut SpeedReturnChannel(PTD4);
LordScarface 8:32133eeb7037 35
LordScarface 9:92283a284936 36
LordScarface 9:92283a284936 37
LordScarface 9:92283a284936 38 //INPUTS____________________
LordScarface 8:32133eeb7037 39
LordScarface 9:92283a284936 40 //von K66F
LordScarface 8:32133eeb7037 41 PwmIn steeringAngle(PTD0);
LordScarface 8:32133eeb7037 42
LordScarface 9:92283a284936 43
LordScarface 8:32133eeb7037 44 PwmIn motorSpeed(PTD5);
LordScarface 8:32133eeb7037 45
LordScarface 9:92283a284936 46 //von Sensoren und Inputs
LordScarface 8:32133eeb7037 47
LordScarface 9:92283a284936 48 //bremssignal von k66f
LordScarface 8:32133eeb7037 49 InterruptIn eBrake(PTD2);
LordScarface 8:32133eeb7037 50
LordScarface 9:92283a284936 51 //RPM Sensor
LordScarface 9:92283a284936 52 InterruptIn RPM_one(PTD7);
LordScarface 8:32133eeb7037 53
LordScarface 9:92283a284936 54 InterruptIn RPM_two(PTD6);
LordScarface 8:32133eeb7037 55
LordScarface 9:92283a284936 56 //externe Potentiometer
LordScarface 9:92283a284936 57 AnalogIn poti(PTC2);
LordScarface 8:32133eeb7037 58
LordScarface 8:32133eeb7037 59 AnalogIn poti2(PTB1);
LordScarface 8:32133eeb7037 60
LordScarface 9:92283a284936 61 //schalter um fahren freizugeben
LordScarface 8:32133eeb7037 62 DigitalIn fast(PTD1);
LordScarface 8:32133eeb7037 63
LordScarface 8:32133eeb7037 64
LordScarface 8:32133eeb7037 65
LordScarface 8:32133eeb7037 66 //Declarations for RPM measurement___________________
LordScarface 8:32133eeb7037 67 Timer RPM_timer_one;
LordScarface 8:32133eeb7037 68 long RPM_spent_time_one = 0;
LordScarface 8:32133eeb7037 69 long RPM_val_one = 0;
LordScarface 8:32133eeb7037 70
LordScarface 8:32133eeb7037 71 Timer RPM_timer_two;
LordScarface 8:32133eeb7037 72 long RPM_spent_time_two = 0;
LordScarface 8:32133eeb7037 73 long RPM_val_two = 0;
LordScarface 8:32133eeb7037 74
LordScarface 8:32133eeb7037 75 const int numReadings_one = 8;
LordScarface 8:32133eeb7037 76 int RPMs_one[numReadings_one];
LordScarface 8:32133eeb7037 77 int readIndex_one = 0;
LordScarface 8:32133eeb7037 78 int total_one = 0;
LordScarface 8:32133eeb7037 79 int averageRPM_one = 0;
LordScarface 8:32133eeb7037 80 long speed_one = 0;
LordScarface 8:32133eeb7037 81
LordScarface 8:32133eeb7037 82 const int numReadings_two = 8;
LordScarface 8:32133eeb7037 83 int RPMs_two[numReadings_two];
LordScarface 8:32133eeb7037 84 int readIndex_two = 0;
LordScarface 8:32133eeb7037 85 int total_two = 0;
LordScarface 8:32133eeb7037 86 int averageRPM_two = 0;
LordScarface 8:32133eeb7037 87 long speed_two = 0;
LordScarface 8:32133eeb7037 88 //__________________________________________________________
LordScarface 8:32133eeb7037 89
LordScarface 8:32133eeb7037 90 //dings
LordScarface 8:32133eeb7037 91 float prev_val = 0;
LordScarface 9:92283a284936 92 bool locked = false;
LordScarface 8:32133eeb7037 93
LordScarface 9:92283a284936 94 int SLAM1 = 0;
LordScarface 9:92283a284936 95 int SLAM2 = 0;
LordScarface 8:32133eeb7037 96
LordScarface 8:32133eeb7037 97 //INTERRUPT SERVICE ROUNTINES_______________________________
LordScarface 8:32133eeb7037 98 void RPM_one_ISR()
LordScarface 8:32133eeb7037 99 {
LordScarface 8:32133eeb7037 100 RPM_one.rise(NULL);
LordScarface 8:32133eeb7037 101 RPM_spent_time_one = RPM_timer_one.read_high_resolution_us();
LordScarface 8:32133eeb7037 102 RPM_timer_one.reset();
LordScarface 8:32133eeb7037 103 RPM_one.rise(&RPM_one_ISR);
LordScarface 9:92283a284936 104 SLAM1++;
LordScarface 8:32133eeb7037 105 }
LordScarface 8:32133eeb7037 106
LordScarface 8:32133eeb7037 107 void RPM_two_ISR()
LordScarface 8:32133eeb7037 108 {
LordScarface 8:32133eeb7037 109 RPM_two.rise(NULL);
LordScarface 8:32133eeb7037 110 RPM_spent_time_two = RPM_timer_two.read_high_resolution_us();
LordScarface 8:32133eeb7037 111 RPM_timer_two.reset();
LordScarface 8:32133eeb7037 112 RPM_two.rise(&RPM_two_ISR);
LordScarface 9:92283a284936 113 SLAM2++;
LordScarface 8:32133eeb7037 114 }
LordScarface 8:32133eeb7037 115
LordScarface 8:32133eeb7037 116 bool brake = false;
LordScarface 9:92283a284936 117
LordScarface 8:32133eeb7037 118 void eBrake_ISR()
LordScarface 8:32133eeb7037 119 {
LordScarface 9:92283a284936 120 //myled = !myled;
LordScarface 8:32133eeb7037 121 brake = true;
LordScarface 8:32133eeb7037 122 }
LordScarface 8:32133eeb7037 123 void eBrake2_ISR()
LordScarface 8:32133eeb7037 124 {
LordScarface 9:92283a284936 125 //myled = !myled;
LordScarface 8:32133eeb7037 126 brake = false;
LordScarface 9:92283a284936 127 locked = false;
LordScarface 8:32133eeb7037 128 }
LordScarface 8:32133eeb7037 129
LordScarface 8:32133eeb7037 130 //___________________________________________________________
LordScarface 8:32133eeb7037 131
LordScarface 8:32133eeb7037 132 //MAPPING FUNCTION
LordScarface 8:32133eeb7037 133 float map(float x, float in_min, float in_max, float out_min, float out_max)
LordScarface 8:32133eeb7037 134 {
LordScarface 8:32133eeb7037 135 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
LordScarface 8:32133eeb7037 136 }
LordScarface 8:32133eeb7037 137
LordScarface 8:32133eeb7037 138 void setRightESC(float val)
LordScarface 8:32133eeb7037 139 {
LordScarface 8:32133eeb7037 140 if (val != prev_val) {
LordScarface 8:32133eeb7037 141 rightESC.write(val);
LordScarface 8:32133eeb7037 142 prev_val = val;
LordScarface 8:32133eeb7037 143 }
LordScarface 8:32133eeb7037 144 }
LordScarface 8:32133eeb7037 145
LordScarface 8:32133eeb7037 146 volatile bool newData = false;
LordScarface 8:32133eeb7037 147 volatile float inputs[3];
LordScarface 8:32133eeb7037 148
LordScarface 8:32133eeb7037 149
LordScarface 8:32133eeb7037 150
LordScarface 8:32133eeb7037 151
LordScarface 8:32133eeb7037 152 Timer speedTimer;
LordScarface 8:32133eeb7037 153
LordScarface 8:32133eeb7037 154 //PID-Stuff-------------------------------------------------------------
LordScarface 9:92283a284936 155
LordScarface 9:92283a284936 156 float set_speed = 1150; //desired RPM value
LordScarface 9:92283a284936 157 float Kp = 0;
LordScarface 8:32133eeb7037 158 float Ki = 0;
LordScarface 8:32133eeb7037 159 float Kd = 0;
LordScarface 9:92283a284936 160
LordScarface 9:92283a284936 161 float leftRPM, rightRPM;
LordScarface 9:92283a284936 162
LordScarface 9:92283a284936 163 float PID_errorL, PID_error_preL, PID_outL, PID_sumL, PID_valL;
LordScarface 9:92283a284936 164
LordScarface 9:92283a284936 165 float PID_errorR, PID_error_preR, PID_outR, PID_sumR, PID_valR;
LordScarface 9:92283a284936 166
LordScarface 9:92283a284936 167
LordScarface 9:92283a284936 168
LordScarface 9:92283a284936 169
LordScarface 9:92283a284936 170
LordScarface 8:32133eeb7037 171 //----------------------------------------------------------------------
LordScarface 8:32133eeb7037 172
LordScarface 9:92283a284936 173 void calcPIDleft()
LordScarface 8:32133eeb7037 174 {
LordScarface 9:92283a284936 175 //save error from last reading
LordScarface 9:92283a284936 176 PID_error_preL = PID_errorL;
LordScarface 9:92283a284936 177
LordScarface 9:92283a284936 178 //calculate actual error
LordScarface 9:92283a284936 179 PID_errorL = set_speed - leftRPM;
LordScarface 9:92283a284936 180
LordScarface 9:92283a284936 181 //basic pid formula
LordScarface 9:92283a284936 182 PID_valL = Kp * PID_errorL + Ki * PID_sumL + Kd * (PID_errorL - PID_error_preL);
LordScarface 9:92283a284936 183
LordScarface 9:92283a284936 184 //store output
LordScarface 9:92283a284936 185 PID_outL = PID_valL;
LordScarface 8:32133eeb7037 186
LordScarface 9:92283a284936 187 //sum up the integral
LordScarface 9:92283a284936 188 PID_sumL += PID_errorL;
LordScarface 8:32133eeb7037 189
LordScarface 9:92283a284936 190 //limit the integral to above and below 500
LordScarface 9:92283a284936 191 if (PID_sumL > 500) {
LordScarface 9:92283a284936 192 PID_sumL = 500;
LordScarface 9:92283a284936 193 } else if (PID_sumL < -500) {
LordScarface 9:92283a284936 194 PID_sumL = -500;
LordScarface 8:32133eeb7037 195 }
LordScarface 8:32133eeb7037 196
LordScarface 9:92283a284936 197
LordScarface 9:92283a284936 198 }
LordScarface 9:92283a284936 199
LordScarface 9:92283a284936 200 void calcPIDright()
LordScarface 9:92283a284936 201 {
LordScarface 9:92283a284936 202 //save error from last reading
LordScarface 9:92283a284936 203 PID_error_preR = PID_errorR;
LordScarface 9:92283a284936 204
LordScarface 9:92283a284936 205 //calculate actual error
LordScarface 9:92283a284936 206 PID_errorR = set_speed - rightRPM;
LordScarface 9:92283a284936 207
LordScarface 9:92283a284936 208 //basic pid formula
LordScarface 9:92283a284936 209 PID_valR = Kp * PID_errorR + Ki * PID_sumR + Kd * (PID_errorR - PID_error_preR);
LordScarface 9:92283a284936 210
LordScarface 9:92283a284936 211 //store output
LordScarface 9:92283a284936 212 PID_outR = PID_valR;
LordScarface 9:92283a284936 213
LordScarface 9:92283a284936 214 //sum up the integral
LordScarface 9:92283a284936 215 PID_sumR += PID_errorR;
LordScarface 9:92283a284936 216
LordScarface 9:92283a284936 217 //limit the integral to above and below 500
LordScarface 9:92283a284936 218 if (PID_sumR > 500) {
LordScarface 9:92283a284936 219 PID_sumR = 500;
LordScarface 9:92283a284936 220 } else if (PID_sumR < -500) {
LordScarface 9:92283a284936 221 PID_sumR = -500;
LordScarface 8:32133eeb7037 222 }
LordScarface 9:92283a284936 223
LordScarface 9:92283a284936 224
LordScarface 8:32133eeb7037 225 }
LordScarface 8:32133eeb7037 226
emilmont 0:f59179afee57 227
LordScarface 9:92283a284936 228
LordScarface 9:92283a284936 229
LordScarface 9:92283a284936 230
LordScarface 9:92283a284936 231
LordScarface 9:92283a284936 232 //ESP variables
LordScarface 9:92283a284936 233
LordScarface 9:92283a284936 234 float ESP_left = 0;
LordScarface 9:92283a284936 235 float ESP_right = 0;
LordScarface 9:92283a284936 236
LordScarface 9:92283a284936 237 //ideal 3
LordScarface 9:92283a284936 238 float gain = 1;
LordScarface 9:92283a284936 239
LordScarface 9:92283a284936 240 float centerRPM = 0;
LordScarface 9:92283a284936 241
LordScarface 9:92283a284936 242 float k66conversion = 0;
LordScarface 9:92283a284936 243
LordScarface 9:92283a284936 244
LordScarface 9:92283a284936 245 float prev_angle = 0;
LordScarface 9:92283a284936 246
LordScarface 9:92283a284936 247 int angle(int pwm){
LordScarface 9:92283a284936 248
LordScarface 9:92283a284936 249 return map(steeringAngle.pulsewidth()*1000000,1000,2000,-45,45);
LordScarface 9:92283a284936 250
LordScarface 9:92283a284936 251 }
LordScarface 9:92283a284936 252
LordScarface 9:92283a284936 253
LordScarface 9:92283a284936 254
LordScarface 9:92283a284936 255
LordScarface 9:92283a284936 256 int beep = 0;
LordScarface 9:92283a284936 257
LordScarface 9:92283a284936 258 float temp1 = 0;
LordScarface 9:92283a284936 259 float temp2 = 0;
LordScarface 9:92283a284936 260
LordScarface 9:92283a284936 261 //______________________MAIN____________________________________________________
LordScarface 9:92283a284936 262
sam_grove 7:986d5298b118 263 int main()
sam_grove 7:986d5298b118 264 {
LordScarface 9:92283a284936 265 pc.baud(115200);
LordScarface 8:32133eeb7037 266 pc.printf("Ready!\n");
LordScarface 8:32133eeb7037 267
LordScarface 9:92283a284936 268
LordScarface 8:32133eeb7037 269
LordScarface 8:32133eeb7037 270 //Settings for Interrrupts for RPM Sensors
LordScarface 8:32133eeb7037 271 RPM_one.rise(&RPM_one_ISR);
LordScarface 8:32133eeb7037 272 RPM_timer_one.start();
LordScarface 8:32133eeb7037 273 RPM_two.rise(&RPM_two_ISR);
LordScarface 8:32133eeb7037 274 RPM_timer_two.start();
LordScarface 8:32133eeb7037 275
LordScarface 8:32133eeb7037 276 //Setting for EBrake Interrrupt
LordScarface 8:32133eeb7037 277 eBrake.rise(&eBrake_ISR);
LordScarface 8:32133eeb7037 278 eBrake.fall(&eBrake2_ISR);
LordScarface 8:32133eeb7037 279
LordScarface 8:32133eeb7037 280 //Values for Servo Passthrough
LordScarface 9:92283a284936 281 float servo_pulsewidth = 0;
LordScarface 9:92283a284936 282 float pre_pulsewidth = 0;
LordScarface 8:32133eeb7037 283
LordScarface 8:32133eeb7037 284 //Servo Settings
LordScarface 8:32133eeb7037 285 Servo.period_ms(20);
LordScarface 8:32133eeb7037 286 //pc.printf("%f",period
LordScarface 9:92283a284936 287
LordScarface 9:92283a284936 288 SpeedReturnChannel.period_ms(10);
sam_grove 7:986d5298b118 289
LordScarface 8:32133eeb7037 290 //ESC Settings
LordScarface 9:92283a284936 291 leftESC.period_ms(10);
LordScarface 9:92283a284936 292 rightESC.period_ms(10);
LordScarface 8:32133eeb7037 293
LordScarface 8:32133eeb7037 294 //ARMING ESCS
LordScarface 8:32133eeb7037 295 leftESC.pulsewidth_us(1500); //write them to idle position
LordScarface 8:32133eeb7037 296 rightESC.pulsewidth_us(1500);
LordScarface 9:92283a284936 297
LordScarface 9:92283a284936 298 //wait 4 seconds to arm escs
LordScarface 9:92283a284936 299 wait(0.9f);
LordScarface 8:32133eeb7037 300 wait(0.9f);
LordScarface 8:32133eeb7037 301 wait(0.2f);
LordScarface 9:92283a284936 302 wait(0.9f);
LordScarface 8:32133eeb7037 303 wait(0.9f);
LordScarface 8:32133eeb7037 304 wait(0.2f);
LordScarface 9:92283a284936 305
LordScarface 9:92283a284936 306 blue = !green;
LordScarface 9:92283a284936 307 green = !red;
LordScarface 9:92283a284936 308 red = !blue;
LordScarface 9:92283a284936 309
LordScarface 8:32133eeb7037 310
LordScarface 8:32133eeb7037 311
LordScarface 8:32133eeb7037 312
LordScarface 8:32133eeb7037 313 speedTimer.start();
LordScarface 8:32133eeb7037 314 Timer ppp;
LordScarface 8:32133eeb7037 315 float poti_val = 0;
LordScarface 8:32133eeb7037 316
LordScarface 8:32133eeb7037 317
LordScarface 8:32133eeb7037 318 //MAIN LOOP_________________________________________________________________
emilmont 1:32eacc4f6beb 319 while (true) {
LordScarface 8:32133eeb7037 320
LordScarface 9:92283a284936 321 if(beep >= 10000){
LordScarface 9:92283a284936 322 buzzer.beep(600,10);
LordScarface 9:92283a284936 323 beep = 0;
LordScarface 9:92283a284936 324 }
LordScarface 9:92283a284936 325 beep++;
LordScarface 8:32133eeb7037 326
LordScarface 8:32133eeb7037 327 //MEASURE RPM___________________________________________________________
LordScarface 8:32133eeb7037 328 total_one = total_one - RPMs_one[readIndex_one];
LordScarface 8:32133eeb7037 329 total_two = total_two - RPMs_two[readIndex_two];
LordScarface 8:32133eeb7037 330
LordScarface 8:32133eeb7037 331 if (RPM_timer_one.read_high_resolution_us() > 500000) {
LordScarface 8:32133eeb7037 332 RPM_val_one = 0;
LordScarface 8:32133eeb7037 333 } else if (RPM_spent_time_one > 0) {
LordScarface 8:32133eeb7037 334 RPM_val_one = 60000000/(RPM_spent_time_one * 8);
LordScarface 8:32133eeb7037 335 } else {
LordScarface 8:32133eeb7037 336 RPM_val_one = 0;
LordScarface 8:32133eeb7037 337 }
LordScarface 8:32133eeb7037 338
LordScarface 8:32133eeb7037 339 if (RPM_timer_two.read_high_resolution_us() > 500000) {
LordScarface 8:32133eeb7037 340 RPM_val_two = 0;
LordScarface 8:32133eeb7037 341 } else if (RPM_spent_time_two > 0) {
LordScarface 8:32133eeb7037 342 RPM_val_two = 60000000/(RPM_spent_time_two * 8);
LordScarface 8:32133eeb7037 343 } else {
LordScarface 8:32133eeb7037 344 RPM_val_two = 0;
LordScarface 8:32133eeb7037 345 }
LordScarface 8:32133eeb7037 346
LordScarface 8:32133eeb7037 347 RPMs_one[readIndex_one] = RPM_val_one;
LordScarface 8:32133eeb7037 348 total_one = total_one + RPMs_one[readIndex_one];
LordScarface 8:32133eeb7037 349 readIndex_one++;
LordScarface 8:32133eeb7037 350
LordScarface 8:32133eeb7037 351 RPMs_two[readIndex_two] = RPM_val_two;
LordScarface 8:32133eeb7037 352 total_two = total_two + RPMs_two[readIndex_two];
LordScarface 8:32133eeb7037 353 readIndex_two++;
LordScarface 8:32133eeb7037 354
LordScarface 8:32133eeb7037 355 //set rpms to zero if under certain speed
LordScarface 8:32133eeb7037 356 if (readIndex_one >= numReadings_one) {
LordScarface 8:32133eeb7037 357 readIndex_one = 0;
LordScarface 8:32133eeb7037 358 }
LordScarface 8:32133eeb7037 359
LordScarface 8:32133eeb7037 360 if (readIndex_two >= numReadings_two) {
LordScarface 8:32133eeb7037 361 readIndex_two = 0;
LordScarface 8:32133eeb7037 362 }
LordScarface 8:32133eeb7037 363
LordScarface 8:32133eeb7037 364 //average the rpms since last run of pid error
LordScarface 8:32133eeb7037 365 averageRPM_one = total_one / numReadings_one;
LordScarface 8:32133eeb7037 366 averageRPM_two = total_two / numReadings_two;
LordScarface 8:32133eeb7037 367
LordScarface 8:32133eeb7037 368 //store pid output
LordScarface 8:32133eeb7037 369 leftRPM = averageRPM_two;
LordScarface 8:32133eeb7037 370 rightRPM = averageRPM_one;
LordScarface 8:32133eeb7037 371
LordScarface 8:32133eeb7037 372 //calculate actual pid error
LordScarface 9:92283a284936 373 calcPIDleft();
LordScarface 9:92283a284936 374 calcPIDright();
LordScarface 9:92283a284936 375
LordScarface 9:92283a284936 376
LordScarface 9:92283a284936 377 //torque vectoring
LordScarface 9:92283a284936 378 ESP_left = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
LordScarface 9:92283a284936 379 ESP_right = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
LordScarface 9:92283a284936 380
LordScarface 9:92283a284936 381 //drifting
LordScarface 9:92283a284936 382 //ESP_right = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
LordScarface 9:92283a284936 383 //ESP_left = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
LordScarface 9:92283a284936 384
LordScarface 9:92283a284936 385
LordScarface 9:92283a284936 386 //groundspeed return channel to K66
LordScarface 9:92283a284936 387 centerRPM = (leftRPM + rightRPM) / 2;
LordScarface 9:92283a284936 388 k66conversion = map( centerRPM , 0 , 8000 , 1000 , 2000 );
LordScarface 9:92283a284936 389 SpeedReturnChannel.pulsewidth_us( k66conversion );
LordScarface 9:92283a284936 390
LordScarface 9:92283a284936 391
LordScarface 9:92283a284936 392
LordScarface 9:92283a284936 393
LordScarface 9:92283a284936 394
LordScarface 8:32133eeb7037 395
LordScarface 8:32133eeb7037 396 //check for drivng switch to be pressed
LordScarface 9:92283a284936 397 if( ( (motorSpeed.pulsewidth()*1000000) <= 1480 || (motorSpeed.pulsewidth()*1000000) >= 1516 ) && !brake && fast) {
LordScarface 9:92283a284936 398
LordScarface 9:92283a284936 399 leftESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outL ) ) + ESP_left);
LordScarface 9:92283a284936 400 rightESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outR ) ) + ESP_right);
LordScarface 9:92283a284936 401 //leftESC.pulsewidth_us(1500 + (int)floor(poti_val));
LordScarface 9:92283a284936 402 //rightESC.pulsewidth_us(1500 + (int)floor(poti_val));
LordScarface 8:32133eeb7037 403 LED = 1;
LordScarface 9:92283a284936 404 leftLed.setGreen();
LordScarface 9:92283a284936 405 rightLed.setGreen();
LordScarface 9:92283a284936 406 //locked = false;
LordScarface 9:92283a284936 407 }else if(brake){
LordScarface 9:92283a284936 408 leftLed.setRed();
LordScarface 9:92283a284936 409 rightLed.setRed();
LordScarface 8:32133eeb7037 410 leftESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 411 rightESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 412 }
LordScarface 8:32133eeb7037 413 else {
LordScarface 9:92283a284936 414 leftLed.setBlue();
LordScarface 9:92283a284936 415 rightLed.setBlue();
LordScarface 9:92283a284936 416
LordScarface 8:32133eeb7037 417 leftESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 418 rightESC.pulsewidth_us(1500);
LordScarface 8:32133eeb7037 419 LED = 0;
LordScarface 8:32133eeb7037 420 }
LordScarface 8:32133eeb7037 421
LordScarface 8:32133eeb7037 422 //PASSTHROUGH FOR STEERING
LordScarface 9:92283a284936 423 pre_pulsewidth = servo_pulsewidth;
LordScarface 9:92283a284936 424
LordScarface 9:92283a284936 425 servo_pulsewidth = steeringAngle.pulsewidth();
LordScarface 9:92283a284936 426 servo_pulsewidth = servo_pulsewidth * 1000000;
LordScarface 9:92283a284936 427
LordScarface 9:92283a284936 428 //noise reduction and filtering of pwm signal from k66
LordScarface 9:92283a284936 429 if( servo_pulsewidth > 2000 ){
LordScarface 9:92283a284936 430 servo_pulsewidth = pre_pulsewidth;
LordScarface 9:92283a284936 431 }
LordScarface 9:92283a284936 432
LordScarface 9:92283a284936 433 Servo.pulsewidth_us(servo_pulsewidth);
LordScarface 9:92283a284936 434
LordScarface 9:92283a284936 435 //poti_val = abs( (poti.read() * 1000) + 1000);
LordScarface 9:92283a284936 436 //Servo.pulsewidth_us( 1500 );
LordScarface 9:92283a284936 437 //SpeedReturnChannel.pulsewidth_us( 1500 );
LordScarface 9:92283a284936 438
LordScarface 9:92283a284936 439
LordScarface 9:92283a284936 440
LordScarface 9:92283a284936 441
LordScarface 9:92283a284936 442
LordScarface 9:92283a284936 443
LordScarface 9:92283a284936 444
LordScarface 9:92283a284936 445
LordScarface 9:92283a284936 446
LordScarface 9:92283a284936 447
LordScarface 9:92283a284936 448 if(pre_pulsewidth != servo_pulsewidth){
LordScarface 9:92283a284936 449 SLAM1 = (SLAM1+SLAM2)/2;
LordScarface 9:92283a284936 450 temp1 = SLAM1*cos(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
LordScarface 9:92283a284936 451 temp2 = SLAM1*sin(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
LordScarface 9:92283a284936 452
LordScarface 9:92283a284936 453 pc.printf("X" "%f" "\n" , temp1 );
LordScarface 9:92283a284936 454 pc.printf("Y" "%f" "\n" , temp2 );
LordScarface 9:92283a284936 455
LordScarface 9:92283a284936 456 SLAM1 = 0;
LordScarface 9:92283a284936 457 SLAM2 = 0;
LordScarface 9:92283a284936 458 }
LordScarface 9:92283a284936 459
LordScarface 9:92283a284936 460
LordScarface 9:92283a284936 461
LordScarface 9:92283a284936 462
LordScarface 9:92283a284936 463
LordScarface 9:92283a284936 464
LordScarface 9:92283a284936 465
LordScarface 9:92283a284936 466
LordScarface 9:92283a284936 467
LordScarface 9:92283a284936 468
LordScarface 9:92283a284936 469
LordScarface 9:92283a284936 470
LordScarface 8:32133eeb7037 471
LordScarface 9:92283a284936 472
LordScarface 9:92283a284936 473 prev_angle += angle(servo_pulsewidth * 1000000);
LordScarface 8:32133eeb7037 474
LordScarface 8:32133eeb7037 475 //poti_val = map( poti.read() , 0 , 1 , 0 , 1 );
LordScarface 9:92283a284936 476
LordScarface 8:32133eeb7037 477
LordScarface 9:92283a284936 478 poti_val = abs(poti.read() * 4000);
LordScarface 9:92283a284936 479
LordScarface 9:92283a284936 480
LordScarface 9:92283a284936 481
LordScarface 9:92283a284936 482
LordScarface 9:92283a284936 483
LordScarface 9:92283a284936 484
LordScarface 9:92283a284936 485
LordScarface 9:92283a284936 486 if( (motorSpeed.pulsewidth()*1000000) <= 1100 ) set_speed = poti_val;
LordScarface 9:92283a284936 487
LordScarface 9:92283a284936 488 if( (motorSpeed.pulsewidth()*1000000) >= 1500 ) set_speed = map(motorSpeed.pulsewidth()*1000000 , 1500 , 2000 , 0 , 7000 );
LordScarface 9:92283a284936 489
LordScarface 8:32133eeb7037 490 //Kp = poti_val;
LordScarface 8:32133eeb7037 491
LordScarface 8:32133eeb7037 492
LordScarface 8:32133eeb7037 493 //DEBUGGING___________________________________
LordScarface 8:32133eeb7037 494 /*
LordScarface 9:92283a284936 495 pc.printf("motorSpeed:");
LordScarface 9:92283a284936 496 pc.printf("%f", motorSpeed.pulsewidth()*1000000);
LordScarface 9:92283a284936 497 pc.printf("\t");
LordScarface 9:92283a284936 498 */
LordScarface 9:92283a284936 499 /*
LordScarface 9:92283a284936 500 pc.printf("leftRPM:");
LordScarface 9:92283a284936 501 pc.printf("%f", leftRPM);
LordScarface 9:92283a284936 502 pc.printf("\t");
LordScarface 9:92283a284936 503
LordScarface 9:92283a284936 504 pc.printf("rightRPM:");
LordScarface 9:92283a284936 505 pc.printf("%f", rightRPM);
LordScarface 9:92283a284936 506 pc.printf("\t");
LordScarface 9:92283a284936 507
LordScarface 9:92283a284936 508 pc.printf("ESP_left:");
LordScarface 9:92283a284936 509 pc.printf("%f", ESP_left);
LordScarface 9:92283a284936 510 pc.printf("\t");
LordScarface 9:92283a284936 511
LordScarface 9:92283a284936 512 pc.printf("ESP_right:");
LordScarface 9:92283a284936 513 pc.printf("%f", ESP_right);
LordScarface 9:92283a284936 514 pc.printf("\t");
LordScarface 9:92283a284936 515
LordScarface 9:92283a284936 516 pc.printf("steering angle:");
LordScarface 9:92283a284936 517 pc.printf("%f", (servo_pulsewidth));
LordScarface 9:92283a284936 518 pc.printf("\t");
LordScarface 9:92283a284936 519
LordScarface 9:92283a284936 520
LordScarface 9:92283a284936 521
LordScarface 9:92283a284936 522 pc.printf("PID_OUT:");
LordScarface 9:92283a284936 523 pc.printf("%f", 1500+PID_outL );
LordScarface 9:92283a284936 524 pc.printf("\t");
LordScarface 9:92283a284936 525
LordScarface 9:92283a284936 526
LordScarface 9:92283a284936 527 pc.printf("set_speed:");
LordScarface 9:92283a284936 528 pc.printf("%f ", set_speed);
LordScarface 9:92283a284936 529 pc.printf("\t");
LordScarface 9:92283a284936 530
LordScarface 9:92283a284936 531
LordScarface 9:92283a284936 532 pc.printf("gain:");
LordScarface 9:92283a284936 533 pc.printf("%f", gain);
LordScarface 9:92283a284936 534 pc.printf("\t");
LordScarface 9:92283a284936 535
LordScarface 9:92283a284936 536
LordScarface 9:92283a284936 537 //necessary to keep values centered
LordScarface 9:92283a284936 538 pc.printf("zero: 0.0 \t \n" );
LordScarface 8:32133eeb7037 539 */
LordScarface 8:32133eeb7037 540 //_____________________________________________
LordScarface 8:32133eeb7037 541
emilmont 0:f59179afee57 542 }
LordScarface 8:32133eeb7037 543 //END OF MAIN LOOP__________________________________________________________
LordScarface 8:32133eeb7037 544 }