#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 関数を使って、タッチセンサの入力を監視すべきでしょう。