Diff: main.cpp

Revision:

0:660e73691ec3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 31 20:25:08 2019 +0000
@@ -0,0 +1,114 @@
+#include "PID.h"
+#include "Motor.h"
+#include <Map.hpp>
+#include "mbed.h"
+ //define pins//
+AnalogIn analog_value1(PA_0);
+AnalogIn analog_value2(PA_1);
+DigitalOut myled(LED1);
+CAN can1(PA_11, PA_12);
+Motor myMotor(PA_8, PA_5, PA_6);
+//define variable//
+int counter = 0;
+int counter1 = 0;
+int errorcounter = 0;
+const float output_lower_limit = -255;          
+const float output_upper_limit = 255;
+const float kp = 1.5;
+const float ki = 1;
+const float kd = 0.001;
+const float Ts = 0.001;
+float  thorttle1, thorttle2, sumThorttle, subThorttle;
+float mdagree, pdagree = 0;
+float mythorttle,sumthorttle = 0;
+float speed = 0;
+PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
+
+///////////////////
+//////////////////
+//define Mapping//
+//////////////////
+////////////////////
+Map mapvaltovolt = Map(0, 1, 0, 3300);
+Map mtodagree = Map(490, 3150, 0, 255);
+Map nspeed = Map(-255, 255, -1, 1);
+Map pedaltodagree = Map(865, 1220, 0, 255);
+//////Tickers/////
+Ticker sensor_sample_timer;
+Ticker motor_cmd_timer;
+Ticker get_data_timer;
+Serial pc(USBTX,USBRX);
+/////////////////////
+////////////////////
+////////////////////
+//define functions///
+////////////////////
+/////////////////////
+/////////////////////
+void commandMotor(){
+    counter1++;
+    if (counter1 == 20){ //getting error everycount = 20
+        //pc.printf("Error %.4f\n\r", pid.getError());
+        counter1 = 0;
+        }
+    float Motorcommand = nspeed.Calculate(speed);
+    myMotor.speed(Motorcommand);
+    //pc.printf("Motor speed %.4f\n\r", Motorcommand); 
+}  
+void sendError() { //sending Error over Canbus
+    myMotor.speed(0);
+    wait(10);
+    myled = 1;
+    }
+void getData() { // get Data overcanbus
+    CANMessage msg;
+        if(can1.read(msg)) {
+            //printf("got it\n\r");
+            //printf("Message received: %d,,\n\r", msg.data[0]);
+            pdagree = msg.data[0];
+            //pid.sample();
+            commandMotor();
+            }
+        }
+                
+  
+
+void readSensors() { //reading Pedal and Brake sensors
+    
+    thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    thorttle1 = mapvaltovolt.Calculate(thorttle1);
+    thorttle2 = mapvaltovolt.Calculate(thorttle2);
+    //pc.printf("pedal1 is: %.4f, matzeret1 is:%.4f\n\r", thorttle1, thorttle2);
+    sumThorttle = thorttle1+thorttle2;
+    //pc.printf("sumpedal is: %.4f\n\r", sumThorttle);
+    subThorttle = abs(3500-sumThorttle);
+    //pc.printf("Subpedal is: %.4f\n\r", subThorttle);
+    if (subThorttle<200) { //checking error
+        mythorttle = mtodagree.Calculate(thorttle1);
+        sumthorttle = sumthorttle + mythorttle;
+        //setpoint = throttleCmd;
+        //pc.printf("setpedal dagree %.4f\n\r", pdagree);
+        counter++;
+        if (counter == 10) {
+            mdagree = sumthorttle/10;
+            getData();
+            counter = 0;
+            sumthorttle = 0;
+            errorcounter = 0;
+        }
+    else {
+        errorcounter++;
+        if (errorcounter > 1000) {
+            sendError();
+            }
+        
+        } 
+        
+    }
+}
+int main() {
+    sensor_sample_timer.attach(readSensors, 0.0001);
+    pid.start();
+    motor_cmd_timer.attach(commandMotor, 0.0005);
+}