dror balbul / Mbed 2 deprecated pidtaurd

taurd for noam

Dependencies:   mbed Map

main.cpp@2:fd1023193cc4, 2019-12-20 (annotated)

Committer:
drorbalbul
Date:
Fri Dec 20 16:29:33 2019 +0000
Revision:
2:fd1023193cc4
Parent:
1:c48bef0d5d3c 
pidtaurd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:85367d3ee779 1 #include "PID.h"
drorbalbul 2:fd1023193cc4 2 #include "Motor.h"
drorbalbul 2:fd1023193cc4 3 #include <Map.hpp>
drorbalbul 2:fd1023193cc4 4 #include "mbed.h"
aberk 0:85367d3ee779 5 #define RATE 0.1
aberk 0:85367d3ee779 6
aberk 0:85367d3ee779 7 //Kc, Ti, Td, interval
drorbalbul 2:fd1023193cc4 8 AnalogIn analog_value1(PA_0);
drorbalbul 2:fd1023193cc4 9 AnalogIn analog_value2(PA_1);
drorbalbul 2:fd1023193cc4 10 AnalogIn analog_value3(PA_3);
drorbalbul 2:fd1023193cc4 11 Motor myMotor(PA_8, PA_5, PA_6);
drorbalbul 2:fd1023193cc4 12 Serial pc(USBTX,USBRX);
aberk 0:85367d3ee779 13
drorbalbul 2:fd1023193cc4 14 pid mypid;
drorbalbul 2:fd1023193cc4 15 pid* throttlePid;
drorbalbul 2:fd1023193cc4 16 float throttleCmd;
drorbalbul 2:fd1023193cc4 17 float throttlePosition;
drorbalbul 2:fd1023193cc4 18 float output;
drorbalbul 2:fd1023193cc4 19 float kp= 2.5;
drorbalbul 2:fd1023193cc4 20 float ki= 0.35;
drorbalbul 2:fd1023193cc4 21 float kd= 0.00;
drorbalbul 2:fd1023193cc4 22 Map mapvaltovolt = Map(0, 1, 0, 3300);
drorbalbul 2:fd1023193cc4 23 Map pedaltodagree = Map(850, 1200, 0, 90);
drorbalbul 2:fd1023193cc4 24 Map mtodagree = Map(490, 2900, 0, 90);
drorbalbul 2:fd1023193cc4 25 Map mapoutput = Map(-255, 255, -1.0, 1.0);
drorbalbul 2:fd1023193cc4 26
drorbalbul 2:fd1023193cc4 27 int speed = 0;
drorbalbul 2:fd1023193cc4 28 int main(){
drorbalbul 2:fd1023193cc4 29
drorbalbul 2:fd1023193cc4 30 //float pedal1, pedal2, matzeret1, matzeret2, sumpedal, summetzeret, subpedal, submetzeret;
drorbalbul 2:fd1023193cc4 31 //int setpedal = 0, setthrotle = 0;
drorbalbul 2:fd1023193cc4 32 float mdagree = 0, pdagree = 0;
drorbalbul 2:fd1023193cc4 33
aberk 0:85367d3ee779 34
aberk 0:85367d3ee779 35 while(1){
drorbalbul 2:fd1023193cc4 36 pedal1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
drorbalbul 2:fd1023193cc4 37 pedal2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
drorbalbul 2:fd1023193cc4 38 matzeret1 = analog_value3.read();
drorbalbul 2:fd1023193cc4 39 pedal1 = mapvaltovolt.Calculate(pedal1);
drorbalbul 2:fd1023193cc4 40 pedal2 = mapvaltovolt.Calculate(pedal2);
drorbalbul 2:fd1023193cc4 41 matzeret1 = mapvaltovolt.Calculate(matzeret1);
drorbalbul 2:fd1023193cc4 42
drorbalbul 2:fd1023193cc4 43 pc.printf("pedal1 is: %.4f, pedal2 is:%.4f\n\r", pedal1, pedal2);
drorbalbul 2:fd1023193cc4 44 sumpedal = pedal1+pedal2;
drorbalbul 2:fd1023193cc4 45 //pc.printf("sumpedal is: %.4f\n\r", sumpedal);
drorbalbul 2:fd1023193cc4 46 subpedal = abs(3500-sumpedal);
drorbalbul 2:fd1023193cc4 47 //pc.printf("Subpedal is: %.4f\n\r", subpedal);
drorbalbul 2:fd1023193cc4 48 if (subpedal<175) {
drorbalbul 2:fd1023193cc4 49 throttleCmd = pedaltodagree.Calculate(pedal1);
drorbalbul 2:fd1023193cc4 50 pc.printf("setpedal dagree %d\n\r", throttleCmd);
drorbalbul 2:fd1023193cc4 51 //Update the process variable (dagree of the throtle).
drorbalbul 2:fd1023193cc4 52 throttlePosition = mtodagree.Calculate(matzeret1);
drorbalbul 2:fd1023193cc4 53 pc.printf("processvalue dagree %d\n\r", throttlePosition);
drorbalbul 2:fd1023193cc4 54
drorbalbul 2:fd1023193cc4 55 throttlePid=pid_create(&mypid, &throttlePosition, &output, &throttleCmd, kp, ki,kd);
drorbalbul 2:fd1023193cc4 56
drorbalbul 2:fd1023193cc4 57 /* pid tuning according to error */
drorbalbul 2:fd1023193cc4 58 //int error=abs((int)(throttleCmd - throttlePosition));
drorbalbul 2:fd1023193cc4 59 if ((output)>=0){
drorbalbul 2:fd1023193cc4 60 pid_tune(throttlePid, 3.1, 0.13, 1.21);
drorbalbul 2:fd1023193cc4 61 }
drorbalbul 2:fd1023193cc4 62 else if ((output)<0){
drorbalbul 2:fd1023193cc4 63 pid_tune(throttlePid, 3.1, 0.13, 1.21);
drorbalbul 2:fd1023193cc4 64 }
drorbalbul 2:fd1023193cc4 65 pid_compute(throttlePid);
drorbalbul 2:fd1023193cc4 66
drorbalbul 2:fd1023193cc4 67 }
drorbalbul 2:fd1023193cc4 68 speed = mapoutput.Calculte(*pid->output);
drorbalbul 2:fd1023193cc4 69 myMotor.speed(speed);
drorbalbul 2:fd1023193cc4 70
aberk 0:85367d3ee779 71 wait(RATE);
aberk 0:85367d3ee779 72 }
aberk 0:85367d3ee779 73
aberk 0:85367d3ee779 74 }