犬のお散歩

ちょっとプログラム書いて、動かしてみました。

PCからプログラムのスタート/ストップの制御がしたいので、コード引っ張ってて、犬のお散歩みたいですけど^^;

簡単なプログラムです。ソナーセンサーで、近くに障害物があるかどうか調べて、障害物があったら、少し下がって右に回避して、前進を続けるという感じです。

task main() {
  init_sensor();  // センサーの初期化
  while (true) {
    go_forward();
    if (get_sonar() <= 30) {
      // 30cm先に障害物があったら300秒間後退、100秒間右回転
      go_backward();
      wait1Msec(300);
      turn_right();
      wait1Msec(100);
      PlaySound(soundBeepBeep);
      wait1Msec(100);
    }
  }
}

センサーを使う場合は、最初に初期化しないといけません。入力ポートが4個あるので、各センサーを1種類ずつ使う場合は、↓みたいにセンサーを挿すポートを決めちゃっても良いかと思います。あと、各センサーごとに値を取得する関数用意してと。

#define TOUCH_SENSOR S1
#define SOUND_SENSOR S2
#define LIGHT_SENSOR S3
#define SONAR_SENSOR S4

void init_sensor() {
  SetSensorType(TOUCH_SENSOR, sensorTouch);
  SetSensorType(SOUND_SENSOR, sensorSoundDB);
  SetSensorType(LIGHT_SENSOR, sensorLightInactive);
  SetSensorType(SONAR_SENSOR, sensorSONAR);
}
int get_sonar() {
  return SensorValue(SONAR_SENSOR);
}

あと、前進後退、左右回転の関数も用意した方がいいかもしれませんね。

#define L_MOTOR motorB
#define R_MOTOR motorC
void motor_stop() {
  motor[L_MOTOR] = 0;
  motor[R_MOTOR] = 0;
}
void go_forward() {
	motor[L_MOTOR] = 75;
	motor[R_MOTOR] = 75;
}
void go_backward() {
	motor[L_MOTOR] = -75;
	motor[R_MOTOR] = -75;
}
void turn_left() {
	motor[L_MOTOR] = 75;
	motor[R_MOTOR] = -75;
}
void turn_right() {
	motor[L_MOTOR] = -75;
	motor[R_MOTOR] = 75;
}

これを改良して、時間指定して回転とかもいいかも。

オフィシャルの関数リファレンスによると、モータがどれくらい回転したのか調べられるそうなので、1000回転分前進するなどもできると思います。

次は、もっとちゃんとしたの作ります。