RASCar
s
Dependencies: PwmIn mbed RASCarLED buzzer millis
Revision 8:32133eeb7037, committed 2020-01-25
- Comitter:
- LordScarface
- Date:
- Sat Jan 25 17:11:19 2020 +0000
- Parent:
- 7:986d5298b118
- Child:
- 9:92283a284936
- Commit message:
- a
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn.lib Sat Jan 25 17:11:19 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/PwmIn/#6d68eb9b6bbb
--- a/main.cpp Wed Jul 16 09:59:53 2014 +0000
+++ b/main.cpp Sat Jan 25 17:11:19 2020 +0000
@@ -1,17 +1,310 @@
#include "mbed.h"
+#include "millis.h"
+#include "PwmIn.h"
+
+
+// I/O Declaration_________________________________________________
DigitalOut myled(LED_GREEN);
+DigitalOut red(LED_RED);
Serial pc(USBTX, USBRX);
+//Serial pc(PTE22, PTE23);
+
+//OUTPUTS
+
+PwmOut leftESC(PTA4);
+
+PwmOut rightESC(PTA5);
+
+PwmOut Servo(PTA12);
+
+DigitalOut LED(PTC1);
+
+
+//INPUTS
+
+PwmIn steeringAngle(PTD0);
+
+PwmIn motorSpeed(PTD5);
+
+
+InterruptIn eBrake(PTD2);
+
+InterruptIn RPM_one(PTD4);
+
+InterruptIn RPM_two(PTA13);
+
+AnalogIn poti(PTB3);
+
+AnalogIn poti2(PTB1);
+
+DigitalIn fast(PTD1);
+
+
+
+//Declarations for RPM measurement___________________
+Timer RPM_timer_one;
+long RPM_spent_time_one = 0;
+long RPM_val_one = 0;
+
+Timer RPM_timer_two;
+long RPM_spent_time_two = 0;
+long RPM_val_two = 0;
+
+const int numReadings_one = 8;
+int RPMs_one[numReadings_one];
+int readIndex_one = 0;
+int total_one = 0;
+int averageRPM_one = 0;
+long speed_one = 0;
+
+const int numReadings_two = 8;
+int RPMs_two[numReadings_two];
+int readIndex_two = 0;
+int total_two = 0;
+int averageRPM_two = 0;
+long speed_two = 0;
+//__________________________________________________________
+
+//dings
+float prev_val = 0;
+
+
+//INTERRUPT SERVICE ROUNTINES_______________________________
+void RPM_one_ISR()
+{
+ RPM_one.rise(NULL);
+ RPM_spent_time_one = RPM_timer_one.read_high_resolution_us();
+ RPM_timer_one.reset();
+ RPM_one.rise(&RPM_one_ISR);
+}
+
+void RPM_two_ISR()
+{
+ RPM_two.rise(NULL);
+ RPM_spent_time_two = RPM_timer_two.read_high_resolution_us();
+ RPM_timer_two.reset();
+ RPM_two.rise(&RPM_two_ISR);
+}
+
+bool brake = false;
+void eBrake_ISR()
+{
+ myled = !myled;
+ brake = true;
+}
+void eBrake2_ISR()
+{
+ myled = !myled;
+ brake = false;
+}
+
+//___________________________________________________________
+
+//MAPPING FUNCTION
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void setRightESC(float val)
+{
+ if (val != prev_val) {
+ rightESC.write(val);
+ prev_val = val;
+ }
+}
+
+volatile bool newData = false;
+volatile float inputs[3];
+
+
+
+
+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 Ki = 0;
+float Kd = 0;
+//----------------------------------------------------------------------
+
+void calcPID()
+{
+ PID_error_pre = PID_error;
+ PID_error = set_speed - leftRPM;
+
+ PID_val = Kp * PID_error + Ki * PID_sum + Kd * (PID_error - PID_error_pre);
+ PID_out += PID_val;
+
+ PID_sum += PID_error;
+
+ if (PID_sum > 500) {
+ PID_sum = 500;
+ } else if (PID_sum < -500) {
+ PID_sum = -500;
+ }
+
+ if (PID_out > 500) {
+ PID_out = 500;
+ } else if (PID_out < -500) {
+ PID_out = -500;
+ }
+}
+
int main()
{
- int i = 0;
- pc.printf("Hello World!\n");
+ pc.printf("Ready!\n");
+
+ pc.baud(115200);
+
+
+ //Settings for Interrrupts for RPM Sensors
+ RPM_one.rise(&RPM_one_ISR);
+ RPM_timer_one.start();
+ RPM_two.rise(&RPM_two_ISR);
+ RPM_timer_two.start();
+
+ //Setting for EBrake Interrrupt
+ eBrake.rise(&eBrake_ISR);
+ eBrake.fall(&eBrake2_ISR);
+
+ //Values for Servo Passthrough
+ float dutycycle = 0;
+ float pulsewidth = 0;
+ float period = 0;
+
+ //Servo Settings
+ period = steeringAngle.period();
+ Servo.period_ms(20);
+ //pc.printf("%f",period
+ //ESC Settings
+ leftESC.period_us(10000);
+ rightESC.period_us(10000);
+
+ //ARMING ESCS
+ leftESC.pulsewidth_us(1500); //write them to idle position
+ rightESC.pulsewidth_us(1500);
+ wait(0.9f); //wait 2 Seconds
+ wait(0.9f);
+ wait(0.2f);
+ wait(0.9f); //wait 2 Seconds
+ wait(0.9f);
+ wait(0.2f);
+
+
+
+
+ speedTimer.start();
+ Timer ppp;
+ float poti_val = 0;
+
+ bool locked = false;
+
+
+ //MAIN LOOP_________________________________________________________________
while (true) {
- wait(0.5f); // wait a small period of time
- pc.printf("%d \n", i); // print the value of variable i
- i++; // increment the variable
- myled = !myled; // toggle a led
+
+
+ //MEASURE RPM___________________________________________________________
+ total_one = total_one - RPMs_one[readIndex_one];
+ total_two = total_two - RPMs_two[readIndex_two];
+
+ if (RPM_timer_one.read_high_resolution_us() > 500000) {
+ RPM_val_one = 0;
+ } else if (RPM_spent_time_one > 0) {
+ RPM_val_one = 60000000/(RPM_spent_time_one * 8);
+ } else {
+ RPM_val_one = 0;
+ }
+
+ if (RPM_timer_two.read_high_resolution_us() > 500000) {
+ RPM_val_two = 0;
+ } else if (RPM_spent_time_two > 0) {
+ RPM_val_two = 60000000/(RPM_spent_time_two * 8);
+ } else {
+ RPM_val_two = 0;
+ }
+
+ RPMs_one[readIndex_one] = RPM_val_one;
+ total_one = total_one + RPMs_one[readIndex_one];
+ readIndex_one++;
+
+ RPMs_two[readIndex_two] = RPM_val_two;
+ total_two = total_two + RPMs_two[readIndex_two];
+ readIndex_two++;
+
+ //set rpms to zero if under certain speed
+ if (readIndex_one >= numReadings_one) {
+ readIndex_one = 0;
+ }
+
+ if (readIndex_two >= numReadings_two) {
+ readIndex_two = 0;
+ }
+
+ //average the rpms since last run of pid error
+ averageRPM_one = total_one / numReadings_one;
+ averageRPM_two = total_two / numReadings_two;
+
+ //store pid output
+ leftRPM = averageRPM_two;
+ rightRPM = averageRPM_one;
+
+ //calculate actual pid error
+ calcPID();
+
+ //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);
+ LED = 1;
+ locked = false;
+ }else if(fast && brake && !locked){
+ locked = true;
+ leftESC.pulsewidth_us(1400);
+ rightESC.pulsewidth_us(1400);
+ wait(0.5f);
+ leftESC.pulsewidth_us(1500);
+ rightESC.pulsewidth_us(1500);
+ }
+ else {
+ leftESC.pulsewidth_us(1500);
+ rightESC.pulsewidth_us(1500);
+ LED = 0;
+ }
+
+ //PASSTHROUGH FOR STEERING
+ pulsewidth = steeringAngle.pulsewidth();
+ Servo.pulsewidth_us(pulsewidth*1000000);
+
+
+ //poti_val = map( poti.read() , 0 , 1 , 0 , 1 );
+ //wait_ms(10);
+
+ poti_val = poti.read() * 500;
+ //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" );
+ */
+ //_____________________________________________
+
}
-}
+ //END OF MAIN LOOP__________________________________________________________
+}
\ No newline at end of file
--- a/mbed.bld Wed Jul 16 09:59:53 2014 +0000 +++ b/mbed.bld Sat Jan 25 17:11:19 2020 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/04dd9b1680ae \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/millis.lib Sat Jan 25 17:11:19 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/hudakz/code/millis/#ac7586424119