Revision 9:92283a284936, committed 2020-05-04

Comitter:

LordScarface

Date:

Mon May 04 22:22:18 2020 +0000

Parent:

8:32133eeb7037

Child:

10:53d54dceea50

Commit message:

mbed studio

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RASCarLED.lib	Mon May 04 22:22:18 2020 +0000
@@ -0,0 +1,1 @@
+RASCarLED#d8a35716b19c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/buzzer.lib	Mon May 04 22:22:18 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Reniboy/code/buzzer/#622b1d533a1c

--- a/main.cpp	Sat Jan 25 17:11:19 2020 +0000
+++ b/main.cpp	Mon May 04 22:22:18 2020 +0000
@@ -1,17 +1,28 @@
 #include "mbed.h"
 #include "millis.h"
 #include "PwmIn.h"
+#include "RASCarLED.h"
+#include "buzzer.h"
 
 
 // I/O Declaration_________________________________________________
 
-DigitalOut myled(LED_GREEN);
+//LEDs
+RASCarLED leftLed(PTC7, PTC0, PTC3);
+RASCarLED rightLed(PTC4, PTC5, PTC6);
+
+DigitalOut green(LED_GREEN);
 DigitalOut red(LED_RED);
-Serial pc(USBTX, USBRX);
-//Serial pc(PTE22, PTE23);
+DigitalOut blue(LED_BLUE);
+
+//Debugging out comment out for either USB or Bluetooth
+//Serial pc(USBTX, USBRX);
+Serial pc(PTE22, PTE23);
 
 //OUTPUTS
 
+Beep buzzer(PTA2);
+
 PwmOut leftESC(PTA4);
 
 PwmOut rightESC(PTA5);
@@ -20,24 +31,34 @@
 
 DigitalOut LED(PTC1);
 
+PwmOut SpeedReturnChannel(PTD4);
 
-//INPUTS
+
+
+//INPUTS____________________
 
+//von K66F
 PwmIn steeringAngle(PTD0);
 
+
 PwmIn motorSpeed(PTD5);
 
+//von Sensoren und Inputs
 
+//bremssignal von k66f
 InterruptIn eBrake(PTD2);
 
-InterruptIn RPM_one(PTD4);
+//RPM Sensor
+InterruptIn RPM_one(PTD7);
 
-InterruptIn RPM_two(PTA13);
+InterruptIn RPM_two(PTD6);
 
-AnalogIn   poti(PTB3);
+//externe Potentiometer
+AnalogIn   poti(PTC2);
 
 AnalogIn   poti2(PTB1);
 
+//schalter um fahren freizugeben
 DigitalIn  fast(PTD1);
 
 
@@ -68,7 +89,10 @@
 
 //dings
 float prev_val = 0;
+bool locked = false;
 
+int SLAM1 = 0;
+int SLAM2 = 0;
 
 //INTERRUPT SERVICE ROUNTINES_______________________________
 void RPM_one_ISR()
@@ -77,6 +101,7 @@
     RPM_spent_time_one = RPM_timer_one.read_high_resolution_us();
     RPM_timer_one.reset();
     RPM_one.rise(&RPM_one_ISR);
+    SLAM1++;
 }
 
 void RPM_two_ISR()
@@ -85,18 +110,21 @@
     RPM_spent_time_two = RPM_timer_two.read_high_resolution_us();
     RPM_timer_two.reset();
     RPM_two.rise(&RPM_two_ISR);
+    SLAM2++;
 }
 
 bool brake = false;
+
 void eBrake_ISR()
 {
-    myled = !myled;
+    //myled = !myled;
     brake = true;
 }
 void eBrake2_ISR()
 {
-    myled = !myled;
+    //myled = !myled;
     brake = false;
+    locked = false;
 }
 
 //___________________________________________________________
@@ -124,44 +152,120 @@
 Timer speedTimer;
 
 //PID-Stuff-------------------------------------------------------------
-float PID_error, PID_error_pre, PID_out, PID_sum, PID_val;
-float leftRPM, rightRPM;
-float set_speed = 300;    //desired RPM value
-float Kp = 1;
+
+float set_speed = 1150;    //desired RPM value
+float Kp = 0;
 float Ki = 0;
 float Kd = 0;
+
+float leftRPM, rightRPM;
+
+float PID_errorL, PID_error_preL, PID_outL, PID_sumL, PID_valL;
+
+float PID_errorR, PID_error_preR, PID_outR, PID_sumR, PID_valR;
+
+
+
+
+
 //----------------------------------------------------------------------
 
-void calcPID()
+void calcPIDleft()
 {
-    PID_error_pre = PID_error;
-    PID_error = set_speed - leftRPM;
+    //save error from last reading
+    PID_error_preL = PID_errorL;
+    
+    //calculate actual error
+    PID_errorL = set_speed - leftRPM;
+    
+    //basic pid formula
+    PID_valL = Kp * PID_errorL + Ki * PID_sumL + Kd * (PID_errorL - PID_error_preL);
+    
+    //store output
+    PID_outL = PID_valL;
 
-    PID_val = Kp * PID_error + Ki * PID_sum + Kd * (PID_error - PID_error_pre);
-    PID_out += PID_val;
-
-    PID_sum += PID_error;
+    //sum up the integral
+    PID_sumL += PID_errorL;
 
-    if (PID_sum > 500) {
-        PID_sum = 500;
-    } else if (PID_sum < -500) {
-        PID_sum = -500;
+    //limit the integral to above and below 500
+    if (PID_sumL > 500) {
+        PID_sumL = 500;
+    } else if (PID_sumL < -500) {
+        PID_sumL = -500;
     }
 
-    if (PID_out > 500) {
-        PID_out = 500;
-    } else if (PID_out < -500) {
-        PID_out = -500;
+
+}
+
+void calcPIDright()
+{
+    //save error from last reading
+    PID_error_preR = PID_errorR;
+    
+    //calculate actual error
+    PID_errorR = set_speed - rightRPM;
+    
+    //basic pid formula
+    PID_valR = Kp * PID_errorR + Ki * PID_sumR + Kd * (PID_errorR - PID_error_preR);
+    
+    //store output
+    PID_outR = PID_valR;
+
+    //sum up the integral
+    PID_sumR += PID_errorR;
+
+    //limit the integral to above and below 500
+    if (PID_sumR > 500) {
+        PID_sumR = 500;
+    } else if (PID_sumR < -500) {
+        PID_sumR = -500;
     }
+
+    
 }
 
 
+
+
+
+
+//ESP variables
+
+float ESP_left = 0;
+float ESP_right = 0;
+
+//ideal 3
+float gain = 1;
+
+float centerRPM = 0;
+
+float k66conversion = 0;
+
+
+float prev_angle = 0;
+
+int angle(int pwm){
+ 
+ return map(steeringAngle.pulsewidth()*1000000,1000,2000,-45,45);
+    
+}    
+
+
+
+
+int beep = 0;
+
+float temp1 = 0;
+float temp2 = 0;
+
+//______________________MAIN____________________________________________________
+
 int main()
 {
+    pc.baud(115200);
     pc.printf("Ready!\n");
 
-    pc.baud(115200);
-
+    
 
     //Settings for Interrrupts for RPM Sensors
     RPM_one.rise(&RPM_one_ISR);
@@ -174,42 +278,51 @@
     eBrake.fall(&eBrake2_ISR);
 
     //Values for Servo Passthrough
-    float dutycycle = 0;
-    float pulsewidth = 0;
-    float period = 0;
+    float servo_pulsewidth = 0;
+    float pre_pulsewidth = 0;
 
     //Servo Settings
-    period = steeringAngle.period();
     Servo.period_ms(20);
     //pc.printf("%f",period
+    
+    SpeedReturnChannel.period_ms(10);
 
     //ESC Settings
-    leftESC.period_us(10000);
-    rightESC.period_us(10000);
+    leftESC.period_ms(10);
+    rightESC.period_ms(10);
 
     //ARMING ESCS
     leftESC.pulsewidth_us(1500);  //write them to idle position
     rightESC.pulsewidth_us(1500);
-    wait(0.9f);   //wait 2 Seconds
+    
+    //wait 4 seconds to arm escs
+    wait(0.9f);   
     wait(0.9f);
     wait(0.2f);
-    wait(0.9f);   //wait 2 Seconds
+    wait(0.9f);   
     wait(0.9f);
     wait(0.2f);
-
+    
+    blue = !green;
+    green = !red;
+    red = !blue;
+    
 
 
 
     speedTimer.start();
     Timer ppp;
     float poti_val = 0;
-    
-    bool locked = false;
 
 
     //MAIN LOOP_________________________________________________________________
     while (true) {
 
+        if(beep >= 10000){
+            buzzer.beep(600,10);
+             beep = 0;    
+        } 
+        beep++;
 
         //MEASURE RPM___________________________________________________________
         total_one = total_one - RPMs_one[readIndex_one];
@@ -257,51 +370,172 @@
         rightRPM = averageRPM_one;
 
         //calculate actual pid error
-        calcPID();
+        calcPIDleft();
+        calcPIDright();
+        
+        
+        //torque vectoring
+        ESP_left = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
+        ESP_right = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
+        
+        //drifting
+        //ESP_right = ( -0.0228 * (servo_pulsewidth) + 35.5581 ) * gain;
+        //ESP_left = ( 0.0228 * (servo_pulsewidth) - 35.5581) * gain;
+        
+                
+        //groundspeed return channel to K66
+        centerRPM = (leftRPM + rightRPM) / 2;
+        k66conversion = map( centerRPM , 0 , 8000 , 1000 , 2000 );
+        SpeedReturnChannel.pulsewidth_us( k66conversion );
+        
+        
+        
+        
+        
 
         //check for drivng switch to be pressed
-        if(fast && !brake) {
-            //leftESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
-            //rightESC.pulsewidth_us((1500 + PID_out < 1500) ? 1500 : (1500 + PID_out > 2000) ? 2000 : 1500 + PID_out);
-            leftESC.pulsewidth_us(1500 + poti_val);
-            rightESC.pulsewidth_us(1500 + poti_val);
+        if( ( (motorSpeed.pulsewidth()*1000000) <= 1480 || (motorSpeed.pulsewidth()*1000000) >= 1516 ) && !brake && fast) {
+            
+            leftESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outL )  ) + ESP_left);
+            rightESC.pulsewidth_us( 1500 + ( ( (0.07518797 * (set_speed ) ) + PID_outR )  ) + ESP_right);
+            //leftESC.pulsewidth_us(1500 + (int)floor(poti_val));
+            //rightESC.pulsewidth_us(1500 + (int)floor(poti_val));
             LED = 1;
-            locked = false;
-        }else if(fast && brake && !locked){
-            locked = true;
-            leftESC.pulsewidth_us(1400);
-            rightESC.pulsewidth_us(1400);
-            wait(0.5f);
+            leftLed.setGreen();
+            rightLed.setGreen();
+            //locked = false;
+        }else if(brake){
+            leftLed.setRed();
+            rightLed.setRed();
             leftESC.pulsewidth_us(1500);
             rightESC.pulsewidth_us(1500);
         }
         else {
+            leftLed.setBlue();
+            rightLed.setBlue();
+            
             leftESC.pulsewidth_us(1500);
             rightESC.pulsewidth_us(1500);
             LED = 0;
         }
 
         //PASSTHROUGH FOR STEERING
-        pulsewidth = steeringAngle.pulsewidth();
-        Servo.pulsewidth_us(pulsewidth*1000000);
+        pre_pulsewidth = servo_pulsewidth;
+        
+        servo_pulsewidth = steeringAngle.pulsewidth();
+        servo_pulsewidth = servo_pulsewidth * 1000000;
+        
+        //noise reduction and filtering of pwm signal from k66
+        if( servo_pulsewidth > 2000 ){
+            servo_pulsewidth = pre_pulsewidth;
+        }    
+        
+        Servo.pulsewidth_us(servo_pulsewidth);
+        
+        //poti_val = abs( (poti.read() * 1000) + 1000);
+        //Servo.pulsewidth_us( 1500 );
+        //SpeedReturnChannel.pulsewidth_us( 1500 );
+        
+       
+        
+       
+      
+       
+       
+       
+       
+       
+       if(pre_pulsewidth != servo_pulsewidth){
+       SLAM1 = (SLAM1+SLAM2)/2;
+       temp1 = SLAM1*cos(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
+       temp2 = SLAM1*sin(angle(servo_pulsewidth * 1000000)+prev_angle)*2;
+      
+       pc.printf("X" "%f" "\n" , temp1 );    
+       pc.printf("Y" "%f" "\n" , temp2 );
+     
+       SLAM1 = 0;
+       SLAM2 = 0;
+       }
+       
+       
+       
+       
+       
+       
+       
+       
+       
+       
+       
+       
 
+        
+        prev_angle += angle(servo_pulsewidth * 1000000);
 
         //poti_val = map( poti.read() , 0 , 1 , 0 , 1 );
-        //wait_ms(10);
+        
 
-        poti_val = poti.read() * 500;
+        poti_val = abs(poti.read() * 4000);
+       
+        
+        
+        
+       
+        
+        
+        if( (motorSpeed.pulsewidth()*1000000) <= 1100 ) set_speed = poti_val;
+        
+        if( (motorSpeed.pulsewidth()*1000000) >= 1500 ) set_speed = map(motorSpeed.pulsewidth()*1000000 , 1500 , 2000 , 0 , 7000 );
+        
         //Kp = poti_val;
         
         
         //DEBUGGING___________________________________
         /*
-        pc.printf("%f ", leftRPM);
-        pc.printf(" , ");
-        //pc.printf("%.5f ", 1500+PID_out );
-        //pc.printf(" , ");
-        pc.printf("%f ,", set_speed);
-        pc.printf("%f ,", poti_val);
-        pc.printf("0.0 \n" );
+        pc.printf("motorSpeed:");
+        pc.printf("%f", motorSpeed.pulsewidth()*1000000);
+        pc.printf("\t");
+        */
+        /*
+        pc.printf("leftRPM:");
+        pc.printf("%f", leftRPM);
+        pc.printf("\t");
+        
+        pc.printf("rightRPM:");
+        pc.printf("%f", rightRPM);
+        pc.printf("\t");
+        
+        pc.printf("ESP_left:");
+        pc.printf("%f", ESP_left);
+        pc.printf("\t");
+        
+        pc.printf("ESP_right:");
+        pc.printf("%f", ESP_right);
+        pc.printf("\t");
+        
+        pc.printf("steering angle:");
+        pc.printf("%f", (servo_pulsewidth));
+        pc.printf("\t");
+        
+        
+        
+        pc.printf("PID_OUT:");
+        pc.printf("%f", 1500+PID_outL );
+        pc.printf("\t");
+        
+        
+        pc.printf("set_speed:");
+        pc.printf("%f ", set_speed);
+        pc.printf("\t");
+        
+        
+        pc.printf("gain:");
+        pc.printf("%f", gain);
+        pc.printf("\t");
+        
+        
+        //necessary to keep values centered
+        pc.printf("zero: 0.0 \t \n" );
         */
         //_____________________________________________