File content as of revision 5:08e214191e2a:

#include "IR.h"
#include "count.h"
/*DigitalIn start_sw(p23);

PwmOut motor_up(p22);
PwmOut motor_down(p21);

DigitalIn encoder_A(p16);
DigitalIn encoder_B(p17);

AnalogIn sensor(p20);

DigitalIn manual_up(p24);
DigitalIn manual_down(p28);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

PwmOut fan(p25);*/

DigitalIn start_sw(p21);

PwmOut motor_up(p23);
PwmOut motor_down(p22);

DigitalIn encoder_A(p16);
DigitalIn encoder_B(p17);

AnalogIn sensor(p20);

DigitalIn manual_up(p28);
DigitalIn manual_down(p24);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

CAN can1(p30,p29);
PwmOut fan(p25);

int main(){
     motor_up.period_us(20);
     motor_down.period_us(20);
     motor_up=0;
     motor_down=0;
     printf("lowest calibration\n\r");
     led1=1;
    A_s.rise(&A_s_rise);
    A_s.fall(&A_s_fall);
    B_s.rise(&B_s_rise);
    B_s.fall(&B_s_fall);
      while(1){
       if(manual_up==1 && manual_down==0){
            while(1){led1=0;
                     motor_up=0.2;
                     motor_down=0;
                     if(sensor>0.5){motor_up=0.2;motor_down=0;wait(1);break;}}
            while(1){led1=1;
                     motor_up=0;
                     motor_down=0.15;
                      if(sensor>0.5){motor_up=0;motor_down=0;break;}}
            break;            
            }                        
       else if(manual_up==0 && manual_down==1){
            while(1){motor_up=0;motor_down=0.15;
                      if(sensor>0.5){motor_up=0;motor_down=0;break;}}}
       else if(start_sw==1){
                break;}
       else{motor_up=0;
                motor_down=0;}
                printf("%f\n\r",(float)sensor);          
        }
        wait(0.3);
 ///////////////real calibration  ///////////////////
        printf("real calibration\n\r");
        led2=1;
     rotation=0;
     int pro=0;
        while(1){
             if(rotation>(pro+15) || rotation<(pro-15)){led1 =! led1;
             pro=rotation;}
           if(manual_up==1 && manual_down==0){
               motor_down=0;
               motor_up=0.15;
               led1=1;
               led2=0;
              printf("1 manual_up,%d\n\r",rotation);
               }
           else if(manual_up==0 && manual_down==1){
               motor_up=0;
               motor_down=0.15;
               printf("1 manual_down,%d\n\r",rotation);
               }
           else if(start_sw==1){
                break;}
           else{motor_up=0;
                motor_down=0;}
                //printf("%f\n\r",(float)sensor);//1130 2490   3364
           }
    rotation=0; 
    led1=0;
    led2=0;
    motor_up=0;
    motor_down=0;
    fan.period_ms(20);
    fan.pulsewidth_us(1100);

IR_data.attach(&cal_distance,0.02);
    printf("connect battery in 5sec please\n\r");
    for(int p=0;p<5;p++){
    led1=1;
    wait(0.6);
    led1=0;
    wait(0.4);} // connect ripo in 5s;
    
    printf("4sec is count down\n\r");    
    for(int p=0;p<4;p++){
    led1=1;
    led2=1;
    wait(0.6);
    led2=0;
    wait(0.4); }
    printf("fan and is running\n\r");
while(1){if(manual_up==1)break;}
int original=avr_distance;
led1=1;
led2=1;
led3=1;
//int original=2350000/IR.read_u16();
double duty;
int d;
int I_d;
int D_d;
int pre_d;
float Kp=0.05;
//float Ki=0;
float Kd=0.01;
int x=0;
int fan_power=1100;
int k=0;
int i=0;
while(1){
    //x=2350000/IR.read_u16();
    x=avr_distance;
    d=x-original;
    I_d +=d;
    D_d=d-pre_d;
////////////////////fan limit///////////////////
   /* if(k==0 && manual_up == 1 && manual_down == 0){
        if(i>=2){i=2;}
        else{
        i++;}
        k=1;
        }
    else if(k==0 && manual_up ==0 && manual_down == 1){
        if(i<=0){i=0;}
        else{i--;}
        k=1;}
      else if(rotation>=3100){  led1=0;
        led2=0;
        led3=0;
        motor_up=0;motor_down=0;fan_power=1100;break;}
    else if(k==1 && manual_up ==0 && manual_down==0){k=0;}
    
     printf("i=%d,",i);
     switch(i){
        case 0:
        fan_power=1100;
        break;
        case 1:
        fan_power=1500;
        break;
        case 2:
        fan_power=1380;
        break;
} 
    // printf("fan_power=%d\n\r",fan_power);*/
     ////////////////////////////////////////////////
    fan.pulsewidth_us(1380);    
  if(rotation>=3100){
        motor_up=0;
        motor_down=0;
        led1=0;
        led2=0;
        led3=0;
        fan.pulsewidth_us(1100);
        break;}
    else if(d<=2 && d>=-1){motor_up=0;motor_down=0;}       
    else{duty=Kp*d+Kd*D_d;     
            if(-0.8<duty && duty<-0.6){motor_up=0.6;motor_down=0;}
       else if(-0.6<=duty && duty<-0.25){motor_up=-duty;motor_down=0;}
       else if(-0.25<=duty && duty<0){motor_up=0.25;motor_down=0;}
       else if(0<=duty && duty<0.25){motor_up=0;motor_down=0.2;}//0.2      
       else if(0.25<=duty && duty<0.6){motor_up=0;motor_down=0.7*duty;}//0.7*duty
       else if(0.6<=duty && duty<0.8){motor_up=0;motor_down=0;}//0.5
       else{motor_up=0;motor_down=0;}
         }
       pre_d=d;
    printf("%d %d %d %f,%f\n\r",x,original,rotation,(float)motor_up,(float)motor_down);  
       }
       motor_up=0;
       motor_down=0;
       fan.pulsewidth_us(1100);
       }
//////////////////////////waiting for start/////////////////////////// 
  /* while(1){
       motor_up.period_us(20);
    motor_down.period_us(20);
       printf("waiting \n\r");
       motor_up=0;
       motor_down=0;
        led1=1;     
      if(start_sw==1){break;}
      }
      wait_ms(300);*/
//////////////////////////calibration start//////////////////////////
/* if(k==0 && manual_up == 1 && manual_down == 0){
        if(fan_power<1500){
        fan_power = fan_power + 90;}
        else{fan_power=1500;}
        data[i]=fan_power;
        i++;
        k=1;
        }
    else if(k==0 && manual_up ==0 && manual_down == 1){
        if(fan_power > 1100){fan_power = fan_power - 20;}
        else{fan_power=1100;}
        data[i]=fan_power;
        i++;
        k=1;}
    else if(k==1 && manual_up ==1 && manual_down == 1){
        led1=0;
        led2=0;
        led3=0;
        motor_up=0;motor_down=0;fan_power=1100;break;}
    else if(k==1 && manual_up ==0 && manual_down==0){k=0;}*/