#include <dmotor.h> #include <unistd.h> #include <dsensor.h> #define MOVE_TIME 2000 #define TURN_TIME 1500 tid_t t_sensor_press,t_square; int square(); // move square funciton int sensor_press(); // sensor press funciton int main(int argc, char *argv[]) { t_sensor_press=execi(&sensor_press,0, NULL,10, DEFAULT_STACK_SIZE); t_square=execi(&square,0, NULL,10, DEFAULT_STACK_SIZE); return 0; } int square(){ // move square funciton motor_a_speed(MAX_SPEED); motor_c_speed(MAX_SPEED); while(1){ motor_a_dir(fwd); motor_c_dir(fwd); msleep(MOVE_TIME); motor_a_dir(fwd); motor_c_dir(rev); msleep(TURN_TIME); } return 0; } int sensor_press(){ // sensor press funciton motor_a_speed(MAX_SPEED); motor_c_speed(MAX_SPEED); while(1){ if(SENSOR_1<0xf000){ motor_a_dir(rev); motor_c_dir(rev); msleep(MOVE_TIME/2); motor_a_dir(fwd); motor_c_dir(rev); msleep(TURN_TIME); } } return 0; } |
![]() |
square()内の90度右折中は30行目のmsleep()が働いています。このとき、タッチセンサーが押されると、sensor_press()内の40行目から始まるif()に処理が移り、後退を始めます。しかし、後退中の43行目のmsleepで処理がsquare()に戻ってしまいます。こうなると、24行目から始まる直進動作に入っていまい、さらに26行目のmsleepでまたまた処理がsensor_press()に移り、45行目から始まる90度右折が始まります。ここの47行目のmsleepで処理がsquare()に戻ってしまい、28行目からの90度右折を開始してしまいます。結局のところ、動作は『90度右折中タッチセンサーON』⇒『後退』⇒『前進』⇒『90度右折』⇒『90度右折』ということになってしまいます。 |
#include <dmotor.h> #include <unistd.h> #include <dsensor.h> #include <semaphore.h> // semaphore function is included #define MOVE_TIME 2000 #define TURN_TIME 1523 tid_t t_sensor_press,t_square; sem_t t_motor; // semaphore variable int sensor_press(); int square(); int main(int argc, char *argv[]) { sem_init(&t_motor,0,1); // initialization of a semaphore t_sensor_press=execi(&sensor_press,0, NULL,10, DEFAULT_STACK_SIZE); t_square=execi(&square,0, NULL,10, DEFAULT_STACK_SIZE); return 0; } int square(){ motor_a_speed(MAX_SPEED); motor_c_speed(MAX_SPEED); while(1){ sem_wait(&t_motor); // acquisition of a semaphore motor_a_dir(fwd); motor_c_dir(fwd); sem_post(&t_motor); // release of a semaphore msleep(MOVE_TIME); sem_wait(&t_motor); // acquisition of a semaphore motor_a_dir(fwd); motor_c_dir(rev); sem_post(&t_motor); // release of a semaphore msleep(TURN_TIME); } return 0; } int sensor_press(){ motor_a_speed(MAX_SPEED); motor_c_speed(MAX_SPEED); while(1){ if(SENSOR_1<0xf000){ sem_wait(&t_motor); // acquisition of a semaphore motor_a_dir(rev); motor_c_dir(rev); msleep(MOVE_TIME/2); motor_a_dir(fwd); motor_c_dir(rev); msleep(TURN_TIME); sem_post(&t_motor); // release of a semaphore } } return 0; } |
なお、このプログラムは、タッチセンサーの値を wake_up 関数を使わずに for ループで監視し続けています。このため、sensor_press タスクの優先度を square タスクよりも大きくしてしまった場合、sensor_press タスクのみが実行されることになってしいます。このプログラムでは優先度が同じですので、ラウンドロビンで両方のタスクが交互に実行されますが、本来は wake_up 関数を使って、タッチセンサの入力を監視すべきでしょう。