1.２足歩行ロボットの開発１

#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <conio.h>

wakeup_t rfoot_off_wakeup(wakeup_t data); // right foot ts off wakeup func
wakeup_t lfoot_off_wakeup(wakeup_t data); // left foot ts off wakeup func
wakeup_t rfoot_on_wakeup(wakeup_t data); // right foot ts on wakeup func
wakeup_t lfoot_on_wakeup(wakeup_t data); // left foot ts on wakeup func
wakeup_t rotation_sensor_wakeup(wakeup_t data); // rotation sensor wakeup func

int main(int argc, char *argv[]){

	motor_a_speed(MAX_SPEED);
	motor_c_speed(MAX_SPEED);

	ds_active(&SENSOR_2);
	ds_rotation_on(&SENSOR_2);

	while(1) {
		cputs("waist");
		ds_rotation_set(&SENSOR_2,0);  // move waist joint
		motor_a_dir(fwd);
		wait_event(&rotation_sensor_wakeup,0);
		motor_a_dir(brake);
		cls();

		cputs("right");
		motor_c_dir(rev);  // move ankle joint
		wait_event(&lfoot_off_wakeup,0);
		motor_c_dir(fwd);
		wait_event(&rfoot_on_wakeup,0);
		sleep(1);
		motor_c_dir(brake);
		cls();

		cputs("waist");
		ds_rotation_set(&SENSOR_2,0);  // move waist joint
		motor_a_dir(fwd);
		wait_event(&rotation_sensor_wakeup,0);
		motor_a_dir(brake);
		cls();

		cputs("left");
		motor_c_dir(rev);  // move ankle joint
		wait_event(&rfoot_off_wakeup,0);
		motor_c_dir(fwd);
		wait_event(&lfoot_on_wakeup,0);
		sleep(1);
		motor_c_dir(brake);
		cls();
	}
}

wakeup_t rfoot_off_wakeup(wakeup_t data){ // right foot ts off wakeup func
	return SENSOR_1>0xf000;
}

wakeup_t lfoot_off_wakeup(wakeup_t data){ // left foot ts off wakeup func
	return SENSOR_3>0xf000;
}

wakeup_t rfoot_on_wakeup(wakeup_t data){ // right foot ts on wakeup func
	return SENSOR_1<0xf000;
}

wakeup_t lfoot_on_wakeup(wakeup_t data){ // left foot ts on wakeup func
	return SENSOR_3<0xf000;
}

wakeup_t rotation_sensor_wakeup(wakeup_t data){ // rotation sensor wakeup func
	return ROTATION_2<-192;
}