robofin

#include "mbed.h"
#include "adrobo.h"
#include "Motor.h"

#define servoi 13    // 追加 サーボモータの分解能 [deg]
#define tmax 1.5    // 追加 motor1,motor2 = 1.0fのとき，機体が180度回転するまでにかかる時間

Motor motor1(MOTOR11, MOTOR12);
Motor motor2(MOTOR21, MOTOR22);
BusOut led(LED1,LED2,LED3,LED4,LED5,LED);
PwmOut servo1(SERVO1);
AnalogIn ad[] = {AD1, AD2};
int main() {
  servo1.period_ms(20);
  int i = 1200;  // for文がないためここで初期値を入れる
  float value;
  int s = 0; //追加　サーボの回転方向変化フラグ
	int q = 0;
	int w =0;
  int flag = 0; //追加　サーボ停止用フラグ　breakを使用する場合は不要
	int flag2 = 0;
  int x = 0;        // 追加　モータを動かすためのフラグ
  double R;     // 追加　目標角度 [deg]
  double motori;    // 追加　機体の回転角の分解能 (1[s]につき何度回転するか) [deg]
  float t;  // 追加　機体を回転させるためにモータを回す時間

  while(1) {
    value = ad[0].read();
        if(value > 0.24){
            flag = 1;   //追加
            // break;
        }else{
            flag = 0;   //追加
						flag2 = 0;
        }
//　ここから追加
				
		if (flag2 == 1){
			i=i;
		}else{
				
		if (w == 0){
    if(flag == 0){
        if(s == 0){ // s = 0なら400から2000, s=1なら2000から400へ100ずつ移動
            i += 100;
        }else if(s == 1){
            i -= 100;
				}
    }else if(flag == 1){    // 10mm以下ならその位置で停止
        i = i;
			  w = 1;
    }        
    if(i >= 2000){
        s = 1;
		} else if(i <= 400){
        s = 0;
    }
    servo1.pulsewidth_us(i);
        wait(0.2);
	}
if (w == 1){
		if(i < 1200){
		  servo1.pulsewidth_us(1200);
		  R = ( (1200 - i) / 100 ) * servoi;     // 目標角度 Rを計算
        
        motori = 180 / tmax;            // motoriを計算
        t = R / motori;             // モータを回す時間を計算(目標角Rをmotoriで割る)
        motor1 = -1.0f;              // 機体を回転　速度は各自変更
        motor2 = -1.0f;             // 速度を変更する際は上のtmaxの値も変えること
        wait(t);                // t[s]回転させる
                  // 0.3[s]前進させる
        motor1 = 0.0f;              // 機体を停止
        motor2 = 0.0f;
		q = 1;
		w = 2;
}else{
		servo1.pulsewidth_us(1200);
		 R = ( (i-1200) / 100 ) * servoi;     // 目標角度 Rを計算
        
        motori = 180 / tmax;            // motoriを計算
        t = R / motori;             // モータを回す時間を計算(目標角Rをmotoriで割る)
        motor1 = 1.0f;              // 機体を回転　速度は各自変更
        motor2 = 1.0f;             // 速度を変更する際は上のtmaxの値も変えること
        wait(t);                // t[s]回転させる
                // 0.3[s]前進させる
        motor1 = 0.0f;              // 機体を停止
        motor2 = 0.0f;
		    
    q = 1;
    w = 2;	
}		
	if(q == 1){
	while(1){
value = ad[0].read();
if (value < 0.5){
  
	  motor1 = -1.0f;              // 機体を前進　速度は各自変更
		motor2 = 1.0f;
	  wait(0.3);
}else{
		motor1 = 0.0f;              // 機体を停止
    motor2 = 0.0f;
	wait(4.0);
  flag = 0;
	w = 0;
	q = 0;
	flag2 = 1;
	break;
		}
	}
}
}
}	
}
	}