yotaro morizumi
/
zoomy_customLibrary
my new gear...
control_theory/pid.cpp@2:e7b09385d197, 2022-03-27 (annotated)
- Committer:
- yootee
- Date:
- Sun Mar 27 04:39:16 2022 +0000
- Revision:
- 2:e7b09385d197
- Parent:
- pid.cpp@0:1456b6f84c75
- Child:
- 11:20418879650b
arrc?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | #include<pid.hpp> |
yootee | 0:1456b6f84c75 | 2 | |
yootee | 0:1456b6f84c75 | 3 | Pid::Pid(double get_Kp,double get_Ki,double get_Kd,int plumi){ |
yootee | 0:1456b6f84c75 | 4 | setGain(get_Kp,get_Ki,get_Kd); |
yootee | 0:1456b6f84c75 | 5 | e_o = 0; |
yootee | 0:1456b6f84c75 | 6 | e_c = 0; |
yootee | 0:1456b6f84c75 | 7 | plusminus=plumi; |
yootee | 0:1456b6f84c75 | 8 | } |
yootee | 0:1456b6f84c75 | 9 | |
yootee | 0:1456b6f84c75 | 10 | double Pid::returnVal(double get_target,double input_val,double time){ |
yootee | 0:1456b6f84c75 | 11 | target = get_target; |
yootee | 0:1456b6f84c75 | 12 | e_c = target - input_val; |
yootee | 0:1456b6f84c75 | 13 | Give_P = Kp * e_c; |
yootee | 0:1456b6f84c75 | 14 | Give_I=Ki*((e_o+e_c)/2.0/time); |
yootee | 0:1456b6f84c75 | 15 | Give_D=Kd*(e_c-e_o)*time; |
yootee | 0:1456b6f84c75 | 16 | e_o = e_c; |
yootee | 0:1456b6f84c75 | 17 | Operation_amount = plusminus * Operation_amount + Give_P+Give_I-Give_D; |
yootee | 0:1456b6f84c75 | 18 | if(Operation_amount > max_val){ |
yootee | 0:1456b6f84c75 | 19 | Operation_amount = max_val; |
yootee | 0:1456b6f84c75 | 20 | }else if(Operation_amount < minimum_val){ |
yootee | 0:1456b6f84c75 | 21 | Operation_amount = minimum_val; |
yootee | 0:1456b6f84c75 | 22 | } |
yootee | 0:1456b6f84c75 | 23 | if(target == input_val){ |
yootee | 0:1456b6f84c75 | 24 | Operation_amount = 0; |
yootee | 0:1456b6f84c75 | 25 | } |
yootee | 0:1456b6f84c75 | 26 | return Operation_amount; |
yootee | 0:1456b6f84c75 | 27 | } |
yootee | 0:1456b6f84c75 | 28 | |
yootee | 0:1456b6f84c75 | 29 | void Pid::setGain(double get_Kp,double get_Ki,double get_Kd){ |
yootee | 0:1456b6f84c75 | 30 | Kp = get_Kp; |
yootee | 0:1456b6f84c75 | 31 | Ki = get_Ki; |
yootee | 0:1456b6f84c75 | 32 | Kd = get_Kd; |
yootee | 0:1456b6f84c75 | 33 | } |
yootee | 0:1456b6f84c75 | 34 | |
yootee | 0:1456b6f84c75 | 35 | void Pid::setMax(double get_max,double get_min){ |
yootee | 0:1456b6f84c75 | 36 | max_val = get_max; |
yootee | 0:1456b6f84c75 | 37 | minimum_val = get_min; |
yootee | 0:1456b6f84c75 | 38 | } |