File content as of revision 0:7a6b6c687fa3:

#include "function.h"

PwmOut pwm11(D5);           //左正転
PwmOut pwm12(D1);           //左逆転
PwmOut pwm21(D6);           //右正転
PwmOut pwm22(D9);           //右逆転
DigitalOut LED(D2);
InterruptIn button1(A2);    //フォトインタラプタ左
InterruptIn button2(A1);    //フォトインタラプタ右
AnalogIn   in1(A6);         //フォトリフレクタ左
AnalogIn   in2(A4);         //フォトリフレクタ真ん中
AnalogIn   in3(A3);         //フォトリフレクタ右

float g = 0.0f;
float gain1 = 0.0f;
float gain2 = 0.0f;
float dif1 = 0.0f;
float dif2 = 0.0f;
float thr1 = 0.40f;
float thr2 = 0.40f;
float thr3 = 0.40f;
int judge1 = 0;
int judge2 = 0;
int judge3 = 0;

int main() {
    pwm11.period(0.01f);  //100Hz
    pwm12.period(0.01f);  //100Hz
    pwm21.period(0.01f);  //100Hz
    pwm22.period(0.01f);  //100Hz
    button1.rise(&flip1);
    button2.rise(&flip2);
    int n;//3biteで状態を表現
    
    flipper.attach(&flip3,0.05);//0.１秒ごとに回転数cを更新
    
    while(1) {
        
        if(  led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
        LED = 1.0f;
        wait(0.5f);
        LED = 0;
        led_count1 = 0;
        led_count2 = 0;
        }
//////////////////floatのinをintのjudgeに変換
        if(in1 > thr1){
            judge1 = 1;
            }
         else{
            judge1 = 0;
            }
            if(in2 > thr2){
            judge2 = 1;
            }
         else{
            judge2 = 0;
            }
            if(in3 > thr3){
            judge3 = 1;
            }
         else{
            judge3 = 0;
            }
/////////////////
        n = judge1 + judge2*2 + judge3*4;
   /*     distance = (distance1 + distance2) / 2;
        if(ditance < 43*12*){
            */
            switch(n){
              case(2)://白黒白ゆっくり進む
              g = 500.0f;
              dif1 = g - c2;
              dif2 = g - c1;
              gain1 = 0.0006f;
              gain2 = 0.0006f;
              pwm11.write(0.45f + dif1*gain1);
              pwm12.write(0.30f);
              pwm21.write(0.45f + dif2*gain2);
              pwm22.write(0.30f);
              break;
              
              case(0)://白白白バック
              g = 500.0f;
              dif1 = g - c1;
              dif2 = g - c2;
              gain1 = 0.001f;
              gain2 = 0.001f;
              
              pwm11.write(0.10f);
              pwm12.write(0.27f +dif1*gain1);
              pwm21.write(0.10f);
              pwm22.write(0.27f +dif2*gain2);
              break;
              
              
            
              case(1)://黒白白左回転かなり強
              g = 500.0f;
              dif1 = g - c1;
              dif2 = g - c2;
              gain1 = 0.0002f;
              gain2 = 0.0002f;
              
              pwm11.write(0.30f + dif1*gain1);
              pwm12.write(0.30f);
              pwm21.write(0.50f + dif2*gain2);
              pwm22.write(0.30f);
              break;
            
              case(4)://白白黒右回転かなり強
              g = 500.0f;
              dif1 = g - c1;
              dif2 = g - c2;
              gain1 = 0.0002f;
              gain2 = 0.0002f;
              
              pwm11.write(0.50f + dif1*gain1);
              pwm12.write(0.30f);
              pwm21.write(0.20f);
              pwm22.write(0.30f + dif2*gain2);
              break;
              
              case(3)://黒黒白左回転強
              g = 500.0f;
              dif1 = g - c1;
              dif2 = g - c2;         
              gain1 = 0.0002f;
              gain2 = 0.0002f;
                  
              pwm11.write(0.00f);
              pwm12.write(0.00f);
              pwm21.write(0.40f + dif2*gain2);
              pwm22.write(0.20f);
              break;
              
              case(6)://白黒黒右回転強
              g = 500.0f;
              dif1 = g - c1;
              dif2 = g - c2; 
              gain1 = 0.0002f;
              gain2 = 0.0002f;
              
              pwm11.write(0.40f + dif1*gain1);
              pwm12.write(0.20f);
              pwm21.write(0.00f);
              pwm22.write(0.00f);
              break;
              
              case(7)://黒黒黒ゆっくり
              dif1 = g - c1;
              dif2 = g - c2;
              gain1 = 0.0004f;
              gain2 = 0.0004f;
              
              pwm11.write(0.45f + dif1*gain1);
              pwm12.write(0.30f);
              pwm21.write(0.45f + dif2*gain2);
              pwm22.write(0.30f);
              break;
               
              default://それ以外(多分ないけど)
              pwm11.write(0.10f);
              pwm12.write(0.70f);
              pwm21.write(0.10f);
              pwm22.write(0.70f);
              break;
              
        }
    }
}