RASCar
s
Dependencies: PwmIn mbed RASCarLED buzzer millis
Diff: main.cpp
- Revision:
- 9:92283a284936
- Parent:
- 8:32133eeb7037
--- 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" );
*/
//_____________________________________________