File content as of revision 4:9ba47e5db1e2:

float DELTA_T = 0.004;
float KP = 0.0;
float KI = 0.0;
float KD = 0.0;

float diff_L[2];
float in_L;

float diff_R[2];
float in_R;


float PID_LIGHT_L(float sensor_val,float target){

    float p,i,d;
    
    diff_L[0] = diff_L[1];
    diff_L[1] = sensor_val - target;
    in_L += (diff_L[1] + diff_L[0])*0.5*DELTA_T;

    p = KP*diff_L[1];
    i = KI* in_L;
    d = KD*(diff_L[1]-diff_L[0])/DELTA_T;
    float ans = p + i + d;
    ans = (ans>1)?1:ans;
    ans = (ans<-1)?-1:ans;
    return(ans);
    }

float PID_LIGHT_R(float sensor_val,float target){

    float p,i,d;
    
    diff_R[0] = diff_R[1];
    diff_R[1] = sensor_val - target;
    in_R += (diff_R[1] + diff_R[0])*0.5*DELTA_T;

    p = KP*diff_R[1];
    i = KI* in_R;
    d = KD*(diff_R[1]-diff_R[0])/DELTA_T;
    float ans = p + i + d;
    ans = (ans>1)?1:ans;
    ans = (ans<-1)?-1:ans;
    return(ans);
    }

float pid_val_R,pid_val_L,Power_R,Power_L;

//timer 制御
void FLASH(){
    float sensor_RIGHT = 1/*analog*/;
    float sensor_LEFT = 1/*analog*/;

    pid_val_R = PID_LIGHT_R(sensor_RIGHT,1/*センサー最大値*/);
    pid_val_L = PID_LIGHT_L(sensor_LEFT,1/*センサー最大値*/);

    Power_R = 0.1+pid_val_R-pid_val_L;
    Power_L = 0.1+pid_val_L-pid_val_R;
    Power_R = (Power_R>1.0)?1.0:Power_R;
    Power_R = (Power_R<0)?0:Power_R;
    Power_L = (Power_L>1.0)?1.0:Power_L;
    Power_L = (Power_L<0)?0:Power_L;
}