Revision 0:33b00fa05201, committed 2019-12-20

Comitter:

drorbalbul

Date:

Fri Dec 20 15:17:18 2019 +0000

Commit message:

noam

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map.lib	Fri Dec 20 15:17:18 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 15:17:18 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/Motor.h	Fri Dec 20 15:17:18 2019 +0000
@@ -0,0 +1,56 @@
+/* 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.
+ */
+
+#ifndef MBED_MOTOR_H
+#define MBED_MOTOR_H
+
+#include "mbed.h"
+
+/** Interface to control a standard DC motor 
+ *
+ * with an H-bridge using a PwmOut and 2 DigitalOuts
+ */
+class Motor {
+public:
+
+    /** Create a motor control interface    
+     *
+     * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed
+     * @param fwd A DigitalOut, set high when the motor should go forward
+     * @param rev A DigitalOut, set high when the motor should go backwards
+     */
+    Motor(PinName pwm, PinName fwd, PinName rev);
+    
+    /** Set the speed of the motor
+     * 
+     * @param speed The speed of the motor as a normalised value between -1.0 and 1.0
+     */
+    void speed(float speed);
+
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.c	Fri Dec 20 15:17:18 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 15:17:18 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
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 20 15:17:18 2019 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "Motor.h"
+#include <Map.hpp>
+ 
+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);
+//DigitalOut led(LED1);
+Serial pc(USBTX,USBRX);
+int main() {
+    float pedal1, pedal2, matzeret1, matzeret2, sumpedal, summetzeret, subpedal, submetzeret;
+    Map mapvaltovolt = Map(0, 1, 0, 3300);
+    Map pedaltodagree = Map(850, 1200, 0, 90);
+    Map mtodagree = Map(490, 2900, 0, 90);
+    float mdagree = 0, pdagree = 0;
+    while(1) {
+        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, 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) {
+            if (pedal1 >=1000){
+                myMotor.speed(0.3);
+                 }
+            else {
+                 myMotor.speed(0);
+                }
+          }
+        else{
+            pc.printf("error\n\r");
+            //myMotor.speed(0);
+        }
+        summetzeret = matzeret1 + matzeret2;
+        submetzeret = abs(3400-summetzeret);
+        pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", matzeret1, matzeret2);
+        pc.printf("dagree of pedal = %d, dagree of metzeeret = %d\n\r", (int)pedaltodagree.Calculate(pedal1), (int)mtodagree.Calculate(matzeret1)); 
+        if (submetzeret>175) {
+          pc.printf("METZERET NOT CORRECT\n\n\n\r");
+          }
+          
+          
+        wait(0.01);
+        }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 20 15:17:18 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file