Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

color.cpp@4:9ba47e5db1e2, 2019-10-05 (annotated)

Committer:
kazuryu
Date:
Sat Oct 05 01:41:37 2019 +0000
Revision:
4:9ba47e5db1e2
Parent:
0:3dc012104243 
B

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazuryu 0:3dc012104243 1 float DELTA_T = 0.004;
kazuryu 0:3dc012104243 2 float KP = 0.0;
kazuryu 0:3dc012104243 3 float KI = 0.0;
kazuryu 0:3dc012104243 4 float KD = 0.0;
kazuryu 0:3dc012104243 5
kazuryu 0:3dc012104243 6 float diff_L[2];
kazuryu 0:3dc012104243 7 float in_L;
kazuryu 0:3dc012104243 8
kazuryu 0:3dc012104243 9 float diff_R[2];
kazuryu 0:3dc012104243 10 float in_R;
kazuryu 0:3dc012104243 11
kazuryu 0:3dc012104243 12
kazuryu 0:3dc012104243 13 float PID_LIGHT_L(float sensor_val,float target){
kazuryu 0:3dc012104243 14
kazuryu 0:3dc012104243 15 float p,i,d;
kazuryu 0:3dc012104243 16
kazuryu 0:3dc012104243 17 diff_L[0] = diff_L[1];
kazuryu 0:3dc012104243 18 diff_L[1] = sensor_val - target;
kazuryu 0:3dc012104243 19 in_L += (diff_L[1] + diff_L[0])*0.5*DELTA_T;
kazuryu 0:3dc012104243 20
kazuryu 0:3dc012104243 21 p = KP*diff_L[1];
kazuryu 0:3dc012104243 22 i = KI* in_L;
kazuryu 0:3dc012104243 23 d = KD*(diff_L[1]-diff_L[0])/DELTA_T;
kazuryu 0:3dc012104243 24 float ans = p + i + d;
kazuryu 0:3dc012104243 25 ans = (ans>1)?1:ans;
kazuryu 0:3dc012104243 26 ans = (ans<-1)?-1:ans;
kazuryu 0:3dc012104243 27 return(ans);
kazuryu 0:3dc012104243 28 }
kazuryu 0:3dc012104243 29
kazuryu 0:3dc012104243 30 float PID_LIGHT_R(float sensor_val,float target){
kazuryu 0:3dc012104243 31
kazuryu 0:3dc012104243 32 float p,i,d;
kazuryu 0:3dc012104243 33
kazuryu 0:3dc012104243 34 diff_R[0] = diff_R[1];
kazuryu 0:3dc012104243 35 diff_R[1] = sensor_val - target;
kazuryu 0:3dc012104243 36 in_R += (diff_R[1] + diff_R[0])*0.5*DELTA_T;
kazuryu 0:3dc012104243 37
kazuryu 0:3dc012104243 38 p = KP*diff_R[1];
kazuryu 0:3dc012104243 39 i = KI* in_R;
kazuryu 0:3dc012104243 40 d = KD*(diff_R[1]-diff_R[0])/DELTA_T;
kazuryu 0:3dc012104243 41 float ans = p + i + d;
kazuryu 0:3dc012104243 42 ans = (ans>1)?1:ans;
kazuryu 0:3dc012104243 43 ans = (ans<-1)?-1:ans;
kazuryu 0:3dc012104243 44 return(ans);
kazuryu 0:3dc012104243 45 }
kazuryu 0:3dc012104243 46
kazuryu 0:3dc012104243 47 float pid_val_R,pid_val_L,Power_R,Power_L;
kazuryu 0:3dc012104243 48
kazuryu 0:3dc012104243 49 //timer 制御
kazuryu 0:3dc012104243 50 void FLASH(){
kazuryu 0:3dc012104243 51 float sensor_RIGHT = 1/*analog*/;
kazuryu 0:3dc012104243 52 float sensor_LEFT = 1/*analog*/;
kazuryu 0:3dc012104243 53
kazuryu 0:3dc012104243 54 pid_val_R = PID_LIGHT_R(sensor_RIGHT,1/*センサー最大値*/);
kazuryu 0:3dc012104243 55 pid_val_L = PID_LIGHT_L(sensor_LEFT,1/*センサー最大値*/);
kazuryu 0:3dc012104243 56
kazuryu 0:3dc012104243 57 Power_R = 0.1+pid_val_R-pid_val_L;
kazuryu 0:3dc012104243 58 Power_L = 0.1+pid_val_L-pid_val_R;
kazuryu 0:3dc012104243 59 Power_R = (Power_R>1.0)?1.0:Power_R;
kazuryu 0:3dc012104243 60 Power_R = (Power_R<0)?0:Power_R;
kazuryu 0:3dc012104243 61 Power_L = (Power_L>1.0)?1.0:Power_L;
kazuryu 0:3dc012104243 62 Power_L = (Power_L<0)?0:Power_L;
kazuryu 0:3dc012104243 63 }