color.cpp

00001 float DELTA_T = 0.004;
00002 float KP = 0.0;
00003 float KI = 0.0;
00004 float KD = 0.0;
00005 
00006 float diff_L[2];
00007 float in_L;
00008 
00009 float diff_R[2];
00010 float in_R;
00011 
00012 
00013 float PID_LIGHT_L(float sensor_val,float target){
00014 
00015     float p,i,d;
00016     
00017     diff_L[0] = diff_L[1];
00018     diff_L[1] = sensor_val - target;
00019     in_L += (diff_L[1] + diff_L[0])*0.5*DELTA_T;
00020 
00021     p = KP*diff_L[1];
00022     i = KI* in_L;
00023     d = KD*(diff_L[1]-diff_L[0])/DELTA_T;
00024     float ans = p + i + d;
00025     ans = (ans>1)?1:ans;
00026     ans = (ans<-1)?-1:ans;
00027     return(ans);
00028     }
00029 
00030 float PID_LIGHT_R(float sensor_val,float target){
00031 
00032     float p,i,d;
00033     
00034     diff_R[0] = diff_R[1];
00035     diff_R[1] = sensor_val - target;
00036     in_R += (diff_R[1] + diff_R[0])*0.5*DELTA_T;
00037 
00038     p = KP*diff_R[1];
00039     i = KI* in_R;
00040     d = KD*(diff_R[1]-diff_R[0])/DELTA_T;
00041     float ans = p + i + d;
00042     ans = (ans>1)?1:ans;
00043     ans = (ans<-1)?-1:ans;
00044     return(ans);
00045     }
00046 
00047 float pid_val_R,pid_val_L,Power_R,Power_L;
00048 
00049 //timer 制御
00050 void FLASH(){
00051     float sensor_RIGHT = 1/*analog*/;
00052     float sensor_LEFT = 1/*analog*/;
00053 
00054     pid_val_R = PID_LIGHT_R(sensor_RIGHT,1/*センサー最大値*/);
00055     pid_val_L = PID_LIGHT_L(sensor_LEFT,1/*センサー最大値*/);
00056 
00057     Power_R = 0.1+pid_val_R-pid_val_L;
00058     Power_L = 0.1+pid_val_L-pid_val_R;
00059     Power_R = (Power_R>1.0)?1.0:Power_R;
00060     Power_R = (Power_R<0)?0:Power_R;
00061     Power_L = (Power_L>1.0)?1.0:Power_L;
00062     Power_L = (Power_L<0)?0:Power_L;
00063 }