ライントレースロボットを作ってみた

画質悪いですが、ようつべに上げてみました。

ソースコードはこんな感じ。

#include "sensorlib.c"
#include "motorlib.c"
#define THRES 45

task main() {
   init_sensor();

   while(true) {
     go_forward();
     if (get_light() < THRES) {
       turn_right();
     } else {
       turn_left();
     }
     go_forward();
   }
}

go_forwardは前進、turn_rightは右回転、turn_leftは左回転です。こういうのはよく使いそうなので、ライブラリ化しました。

#define L_MOTOR motorB
#define R_MOTOR motorC

void go_forward() {
	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;
}

光以外は省略してますけど、センサー周りもライブラリ化しました。

#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, sensorLightActive);
  SetSensorType(SONAR_SENSOR, sensorSONAR);
}
int get_light() {
	return SensorValue(LIGHT_SENSOR);
}

ダンシングロボットとかのビデオも上げてみようかな。