Diff: main.cpp

Revision:

0:1825e249cba5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 27 17:29:23 2019 +0000
@@ -0,0 +1,90 @@
+#include "PID.h"
+#include "Motor.h"
+#include <Map.hpp>
+#include "mbed.h"
+#define RATE 0.1
+ 
+AnalogIn analog_value1(PA_0);
+AnalogIn analog_value2(PA_1);
+AnalogIn analog_value3(PA_3);
+AnalogIn analog_value4(PA_4);
+Motor myMotor(PA_8, PA_5, PA_6);
+int counter = 0;
+int counter1 = 0;
+//static float setpoint, feedback, output;
+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 pedal1, pedal2, matzeret1, matzeret2, sumpedal, summetzeret, subpedal, submetzeret;
+float mdagree = 0, pdagree = 0;
+float mypedal,sumPedal,mythorttle,sumthorttle = 0;
+float speed = 0;
+Map mapvaltovolt = Map(0, 1, 0, 3300);
+Map pedaltodagree = Map(865, 1220, 0, 255);
+Map mtodagree = Map(490, 3150, 0, 255);
+Map nspeed = Map(-255, 255, -1, 1);
+PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
+Ticker sensor_sample_timer;
+Ticker motor_cmd_timer;
+
+//DigitalOut led(LED1);
+Serial pc(USBTX,USBRX);
+void commandMotor(){
+    counter1++;
+    if (counter1 == 20){
+        pc.printf("Error %.4f\n\r", pid.getError());
+        counter1 = 0;
+        }
+    float Motorcommand = nspeed.Calculate(speed);
+    //pc.printf("Motor speed %.4f\n\r", Motorcommand);
+    if (Motorcommand > 1) {
+        myMotor.speed(1);
+    }
+    else {
+        myMotor.speed(Motorcommand);
+    } 
+}
+void readSensors(){
+    
+    pedal1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    pedal2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    matzeret1 = analog_value3.read();
+    pedal1 = mapvaltovolt.Calculate(pedal1);
+    pedal2 = mapvaltovolt.Calculate(pedal2);
+    matzeret1 = mapvaltovolt.Calculate(matzeret1);
+    /*matzeret2 = analog_value4.read();
+    matzeret2 = matzeret2 * ConvertToVolt; */
+    //pc.printf("pedal1 is: %.4f, matzeret1 is:%.4f\n\r", pedal1, matzeret1);
+    sumpedal = pedal1+pedal2;
+    //pc.printf("sumpedal is: %.4f\n\r", sumpedal);
+    subpedal = abs(3500-sumpedal);
+    //pc.printf("Subpedal is: %.4f\n\r", subpedal);
+    if (subpedal<175) {
+        mypedal = pedaltodagree.Calculate(pedal1);
+        sumPedal = sumPedal + mypedal;
+        //setpoint = throttleCmd;
+        //pc.printf("setpedal dagree %.4f\n\r", pdagree);
+        mythorttle = mtodagree.Calculate(matzeret1);
+        sumthorttle = sumthorttle + mythorttle;
+        //feedback = throttlePosition;
+        //pc.printf("processvalue dagree %.4f\n\r", mdagree);
+        counter++;
+        if (counter == 10) {
+            pdagree = sumPedal/10;
+            mdagree = sumthorttle/10;
+            counter = 0;
+            sumPedal = 0;
+            sumthorttle = 0;
+        }  
+        
+    }
+}
+int main() {
+    sensor_sample_timer.attach(readSensors, 0.0001);
+    pid.start();
+    motor_cmd_timer.attach(commandMotor, 0.0005);
+}
+