Revision 2:fd1023193cc4, committed 2019-12-20

Comitter:

drorbalbul

Date:

Fri Dec 20 16:29:33 2019 +0000

Parent:

1:c48bef0d5d3c

Commit message:

pidtaurd

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map.lib	Fri Dec 20 16:29:33 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Generic/code/Map/#dad975e2e150

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Fri Dec 20 16:29:33 2019 +0000
@@ -0,0 +1,46 @@
+/* mbed simple H-bridge motor controller
+ * Copyright (c) 2007-2010, sford, http://mbed.org
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Motor.h"
+
+#include "mbed.h"
+
+Motor::Motor(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+    // Set initial condition of PWM
+    _pwm.period(0.001);
+    _pwm = 0;
+
+    // Initial condition of output enables
+    _fwd = 0;
+    _rev = 0;
+}
+
+void Motor::speed(float speed) {
+    _fwd = (speed > 0.0);
+    _rev = (speed < 0.0);
+    _pwm = abs(speed);
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.c	Fri Dec 20 16:29:33 2019 +0000
@@ -0,0 +1,83 @@
+
+#include "PID.h"
+
+pid* pid_create(pid* pid, float* input, float* output, float* setpoint, float kp, float ki, float kd)
+{
+	pid->input = input;
+	pid->output = output;
+	pid->setpoint = setpoint;
+	pid->sumError = 0;
+	pid->lastInput = 0;
+	pid->lastError = 0;
+	pid_limits(pid,-254, 254);
+
+	pid_tune(pid, kp, ki, kd);
+
+	return pid;
+}
+
+void pid_compute(pid* pid)
+{
+
+	
+	float in = *(pid->input);
+	// Compute error
+	float error =(*(pid->setpoint)) - in;
+	// Compute integral
+	pid->sumError += (pid->Ki * error);
+	/*if ((error>0 && pid->lastError<0) || (error<0 && pid->lastError>0) ){
+		pid->sumError = 0;
+	}*/
+	if (pid->sumError > 254)
+		pid->sumError = 255;
+	else if (pid->sumError < -254)
+		pid->sumError = -255;
+	// Compute differential on input
+	float dinput = in - pid->lastInput;
+	// Compute PID output
+	float out = pid->Kp * error + pid->sumError - pid->Kd * dinput;
+	// Apply limit to output value
+	int absOut=abs((int)out);
+	if (out > pid->maxOutput){
+		out = 255;
+		//pid->sumError=0;
+	}
+	else if (out < pid->minOutput){
+		out = -255;
+		//pid->sumError=0;
+	}
+	// Output to pointed variable
+	(*pid->output) = out;
+	// Keep track of some variables for next execution
+	pid->lastInput = in;
+	pid->lastError = error;
+}
+
+void pid_tune(pid* pid, float kp, float ki, float kd)
+{
+	// Check for validity
+	pid->Kp = kp;
+	pid->Ki = ki ;
+	pid->Kd = kd ;
+
+}
+
+
+void pid_limits(pid* pid, float min, float max)
+{
+	if (min >= max) return;
+	pid->minOutput = min;
+	pid->maxOutput = max;
+	if (pid->sumError > pid->maxOutput)
+			pid->sumError = pid->maxOutput;
+	else if (pid->sumError < pid->minOutput)
+			pid->sumError = pid->minOutput;
+}
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Fri Dec 20 16:29:33 2019 +0000
@@ -0,0 +1,48 @@
+
+#ifndef PID_H
+#define PID_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct
+{	// Input, output and setpoint
+	float * input; 
+	float * output; 
+	float * setpoint; 
+	// Tuning parameters
+	float Kp; 
+	float Ki; 
+	float Kd; 
+	// Output minimum and maximum values
+	float minOutput; 
+	float maxOutput; 
+	// Variables for PID algorithm
+	float sumError; 
+	float lastInput; 
+	float lastError;
+}pid;
+
+#ifdef	__cplusplus
+extern "C" {
+#endif
+
+	pid* pid_create(pid* pid, float* input, float* output, float* setoint, float kp, float ki, float kd);
+
+	void pid_compute(pid* pid);
+
+	
+	void pid_tune(pid* pid, float kp, float ki, float kd);
+
+
+	void pid_limits(pid* pid, float min, float max);
+
+
+
+#ifdef	__cplusplus
+}
+#endif
+
+#endif
+// End of Header file
+

--- a/PID.lib	Sat Nov 27 11:32:13 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- 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);
   }