Diff: main.cpp

Revision:

2:fd1023193cc4

Parent:

1:c48bef0d5d3c

--- a/main.cpp	Sat Nov 27 11:32:13 2010 +0000
+++ b/main.cpp	Fri Dec 20 16:29:33 2019 +0000
@@ -1,30 +1,73 @@
 #include "PID.h"
-
+#include "Motor.h"
+#include <Map.hpp>
+#include "mbed.h"
 #define RATE 0.1
 
 //Kc, Ti, Td, interval
-PID controller(1.0, 0.0, 0.0, RATE);
-AnalogIn pv(p15);
-PwmOut   co(p26);
-
-int main(){
+AnalogIn analog_value1(PA_0);
+AnalogIn analog_value2(PA_1);
+AnalogIn analog_value3(PA_3);
+Motor myMotor(PA_8, PA_5, PA_6);
+Serial pc(USBTX,USBRX);
 
-  //Analog input from 0.0 to 3.3V
-  controller.setInputLimits(0.0, 3.3);
-  //Pwm output from 0.0 to 1.0
-  controller.setOutputLimits(0.0, 1.0);
-  //If there's a bias.
-  controller.setBias(0.3);
-  controller.setMode(AUTO_MODE);
-  //We want the process variable to be 1.7V
-  controller.setSetPoint(1.7);
+pid mypid;
+pid* throttlePid;
+float throttleCmd;
+float throttlePosition;
+float output;
+float kp= 2.5;
+float ki= 0.35;
+float kd= 0.00;
+Map mapvaltovolt = Map(0, 1, 0, 3300);
+Map pedaltodagree = Map(850, 1200, 0, 90);
+Map mtodagree = Map(490, 2900, 0, 90);
+Map mapoutput = Map(-255, 255, -1.0, 1.0);
+
+int speed = 0;
+int main(){
+  
+  //float pedal1, pedal2, matzeret1, matzeret2, sumpedal, summetzeret, subpedal, submetzeret;
+  //int setpedal = 0, setthrotle = 0;
+  float mdagree = 0, pdagree = 0;
+  
 
   while(1){
-    //Update the process variable.
-    controller.setProcessValue(pv.read());
-    //Set the new output.
-    co = controller.compute();
-    //Wait for another loop calculation.
+    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);
+    
+    pc.printf("pedal1 is: %.4f, pedal2 is:%.4f\n\r", pedal1, pedal2);
+    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) {
+        throttleCmd = pedaltodagree.Calculate(pedal1);
+        pc.printf("setpedal dagree %d\n\r", throttleCmd);
+        //Update the process variable (dagree of the throtle).
+        throttlePosition = mtodagree.Calculate(matzeret1);
+        pc.printf("processvalue dagree %d\n\r", throttlePosition);
+        
+        throttlePid=pid_create(&mypid, &throttlePosition, &output, &throttleCmd, kp, ki,kd);
+
+        /* pid tuning according to error */
+        //int error=abs((int)(throttleCmd - throttlePosition));
+        if ((output)>=0){
+            pid_tune(throttlePid, 3.1, 0.13, 1.21);
+        }
+        else if ((output)<0){
+            pid_tune(throttlePid, 3.1, 0.13, 1.21);
+        }
+        pid_compute(throttlePid);
+        
+    }
+    speed = mapoutput.Calculte(*pid->output);
+    myMotor.speed(speed);
+    
     wait(RATE);
   }