ﾁｰｾﾏ､ﾞ､ﾇ､ﾎ･鬣､･ﾈ･�｡ｼ･ｹ､ﾏ｡｢ｾ�､ﾋ･愠ﾜ･ﾃ･ﾈ､ｬﾀ軏ｷ､ﾆ､､､�､ｿ､癸｢･ｸ･ｰ･ｶ･ｰ､ﾋﾁﾔ､ｷ､ﾆ､ｷ､ﾞ､､｡｢､｢､ﾞ､�ﾂｮ､ｯ､ﾏﾁ�､ﾞ､ｻ､ﾇ､ｷ､ｿ｡｣
ｺ｣ｲﾏ､ｽ､�､ﾎﾉ､ｷ｡｢ﾂｮ､ｯ｡｢ｰﾂﾄ熙ｷ､ｿﾁ熙ﾂｸｽ､ｷ､ﾞ､ｷ､遉ｦ｡｣
･愠ﾜ･ﾃ･ﾈ､ﾏｴﾜﾅｪ､ﾋ､ｳ､�､ﾞ､ﾇ､ﾎ､筅ﾎ､妤ｭﾂｳ､ｭｻﾈ､､､ﾞ､ｹ｡｣
1｡･ﾈ賽翅ｩｸ�
､ｳ､�､ﾞ､ﾇ､ﾎ･ﾗ･愠ｰ･鬣爨ﾇ､ﾏ｡｢ｿｧ､ﾎ�酖ﾍ､隍�ｰﾅ､､､ﾈ､ｭ｡｢ﾌﾀ､�､､､ﾈ､ｭ｡｢､ﾇﾀ軏ｸ､ﾘﾄ熙ｷ､ﾆ､､､ﾞ､ｷ､ｿ｡｣
､ｷ､ｫ､ｷ｡｢ｸｻ･ｵ､ｬｿｧ､ﾎ�酖ﾍﾉﾕｶ皃ﾉ､ﾟｼ隍ﾃ､ﾆ､､､�､ﾈ､ｭ｡｢･愠ﾜ･ﾃ･ﾈ､遉ｭ､ｯﾀ軏ｹ､�ﾉｬﾍﾗ､ﾏ､｢､熙ﾞ､ｻ､｣
､ｳ､�､酖ﾍ｡ﾊﾌﾜﾉｸﾃﾍ｡ﾋ､ｫ､鬢ﾎ､ｺ､�､ﾋﾈ賽网ｷ､ﾆ｡｢ｺｸｱｦ､ﾎｲｾｺｹ｡ﾊﾀ軏ﾍ｡ﾋ､ｬﾂ遉ｭ､ｯ､ﾊ､�､隍ｦ､ﾋ､ｹ､�､ﾐ｡｢･愠ﾜ･ﾃ･ﾈ､ﾏ､筅ﾃ､ﾈｰﾂﾄ熙ｷ､ﾆﾁ�､�､ﾏ､ｺ､ﾇ､ｹ｡ﾊｿﾞ｡ﾋ｡｣
､ｳ､ﾎ､隍ｦ､ﾊﾀｩｸ讀賽翅ｩｸ�(Pﾀｩｸ�)､ﾈ､､､､､ﾞ､ｹ｡｣
ｼ｡､ﾎ･ﾗ･愠ｰ･鬣爨XT､ﾋ･ﾀ･ｦ･悅ｼ･ﾉ､ｷ､ﾆ､ｯ､ﾀ､ｵ､､｡｣
､ｷ､ｫ､ｷ｡｢ｸｻ･ｵ､ｬｿｧ､ﾎ�酖ﾍﾉﾕｶ皃ﾉ､ﾟｼ隍ﾃ､ﾆ､､､�､ﾈ､ｭ｡｢･愠ﾜ･ﾃ･ﾈ､遉ｭ､ｯﾀ軏ｹ､�ﾉｬﾍﾗ､ﾏ､｢､熙ﾞ､ｻ､｣
､ｳ､�､酖ﾍ｡ﾊﾌﾜﾉｸﾃﾍ｡ﾋ､ｫ､鬢ﾎ､ｺ､�､ﾋﾈ賽网ｷ､ﾆ｡｢ｺｸｱｦ､ﾎｲｾｺｹ｡ﾊﾀ軏ﾍ｡ﾋ､ｬﾂ遉ｭ､ｯ､ﾊ､�､隍ｦ､ﾋ､ｹ､�､ﾐ｡｢･愠ﾜ･ﾃ･ﾈ､ﾏ､筅ﾃ､ﾈｰﾂﾄ熙ｷ､ﾆﾁ�､�､ﾏ､ｺ､ﾇ､ｹ｡ﾊｿﾞ｡ﾋ｡｣
､ｳ､ﾎ､隍ｦ､ﾊﾀｩｸ讀賽翅ｩｸ�(Pﾀｩｸ�)､ﾈ､､､､､ﾞ､ｹ｡｣
ｼ｡､ﾎ･ﾗ･愠ｰ･鬣爨XT､ﾋ･ﾀ･ｦ･悅ｼ･ﾉ､ｷ､ﾆ､ｯ､ﾀ､ｵ､､｡｣
myrobot_interface.h
#define PORT_LIGHT NXT_PORT_S1 /* ﾆｽﾐﾎﾏ･ﾝ｡ｼ･ﾈ､ﾎﾄ�ｵﾁ */ #define PORT_TOUCH NXT_PORT_S2 #define L_MOTOR NXT_PORT_B #define R_MOTOR NXT_PORT_C |
trace_p.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "myrobot_interface.h" /* ｺ鋿ｮ､ｷ､ｿ･ﾘ･ﾃ･ﾀ｡ｼ･ﾕ･｡･､･�､､･ｯ･�｡ｼ･ﾉ */
DeclareTask(Task1); /* Task1､�ｸﾀ */
void ecrobot_device_initialize(){ /* OSEKｵｯﾆｰｻ､ﾎｽ靉 */
nxt_motor_set_speed(L_MOTOR,0,0);
nxt_motor_set_speed(R_MOTOR,0,0);
ecrobot_set_light_sensor_active(PORT_LIGHT);
}
void ecrobot_device_terminate(){ /* OSEKｽｪﾎｻｻ､ﾎｽ靉 */
nxt_motor_set_speed(L_MOTOR,0,0);
nxt_motor_set_speed(R_MOTOR,0,0);
ecrobot_set_light_sensor_inactive(PORT_LIGHT);
}
void user_1ms_isr_type2(void){}
void sound_beep(){ /* ･ﾓ｡ｼ･ﾗｲｻ､ﾄ､鬢ｹ･譯ｼ･ｶｴﾘｿ� */
ecrobot_sound_tone(600, 2, 50);
systick_wait_ms(20);
ecrobot_sound_tone(500, 5, 50);
systick_wait_ms(50);
}
void calibration(int *black,int *white,int *center){ /* �酖ﾍﾀﾟﾄ�ﾍﾑ･譯ｼ･ｶｴﾘｿ� */
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* ､筅ｷｲ｡､ｷ､ｿ､ﾈ､ｭｵｶ */
*black=ecrobot_get_light_sensor(PORT_LIGHT); /* ｹﾎﾌﾀ､�､ｵﾆﾉ､ﾟｼ隍� */
display_clear(0); /* ｲ靂ﾌﾉｽｼｨ */
display_goto_xy(0, 1);
display_string("BLACK=");
display_int(*black, 4);
display_update();
systick_wait_ms(10);
}
sound_beep();
systick_wait_ms(1000);
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* ､筅ｷｲ｡､ｷ､ｿ､ﾈ､ｭｵｶ */
*white=ecrobot_get_light_sensor(PORT_LIGHT); /* ﾇﾎﾌﾀ､�､ｵﾆﾉ､ﾟｼ隍� */
display_clear(0); /* ｲ靂ﾌﾉｽｼｨ */
display_goto_xy(0, 1);
display_string("WHITE=");
display_int(*white, 4);
display_update();
systick_wait_ms(10);
}
sound_beep();
systick_wait_ms(1000);
*center=(*black+*white)/2; /* ﾃ豢ﾖﾃﾍｻｻｽﾐ */
display_clear(0);
display_goto_xy(0, 1);
display_string("CENTER=");
display_int(*center, 4);
display_update();
sound_beep();
systick_wait_ms(1000);
}
void robot_run(int speed,int turn){ /* ･愠ﾜ･ﾃ･ﾈ､鬢ｻ､�･譯ｼ･ｶｴﾘｿ� */
nxt_motor_set_speed(L_MOTOR,speed-turn,1);
nxt_motor_set_speed(R_MOTOR,speed+turn,1);
}
TASK(Task1)
{
int speed=50; /* ﾂｮﾅﾙﾊﾑｿ� */
float p_gain=1.0; /* P･ｲ･､･ﾑｿ� */
int black,white,center; /* ｿｧﾊﾑｿ� */
float turn; /* ﾀ軏ﾍﾊﾑｿ� */
while(1){
calibration(&black,&white,¢er); /* �酖ﾍﾀﾟﾄ�ﾍﾑｴﾘｿﾆ､ﾓｽﾐ､ｷ */
systick_wait_ms(1000); /* 1ﾉﾃﾂﾔ､ﾄ */
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* ､筅ｷｲ｡､ｷ､ｿ､ﾈ､ｭ･ｹ･ｿ｡ｼ･ﾈ */
display_goto_xy(0, 1);
display_string("PUSH START");
display_update();
}
systick_wait_ms(10);
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){
turn = p_gain * (ecrobot_get_light_sensor(PORT_LIGHT) - center)*100 / (black-white);
robot_run(speed, turn); /* ﾁ� */
}
nxt_motor_set_speed(L_MOTOR,0,1);
nxt_motor_set_speed(R_MOTOR,0,1);
systick_wait_ms(1000); /* 1ﾉﾃﾂﾔ､ﾄ */
}
TerminateTask(); /* ｽ靉ｽｪﾎｻ */
}
|
ｾ蠏ｭ､ﾎ･ﾗ･愠ｰ･鬣爨ﾇ､ﾏｲｼｼｰ､ﾎ､隍ｦ､ﾋﾀ軏ﾍ､ﾗｻｻ､ｷ､ﾆ､､､ﾞ､ｹ｡｣
ｭ｡､ﾎｹ爨ﾏﾈ賽罕ｲ･､･ﾇ､ｹ｡｣ｾ螟ﾎ･ｰ･鬣ﾕ､ﾇ､ﾎｷｹ､ｭ､ｽ､ｷ､ﾆ､､､ﾞ､ｹ｡｣
ｭ｢､ﾎｹ爨ﾏﾊﾐｺｹ､ﾇ､ｹ｡｣ｽﾐﾎﾏﾃﾍ､ｬｭ｢､ﾎｹ爨ﾋﾈ賽网ｹ､�､ﾎ､ｬﾈ賽翅ｩｸ讀ﾇ､ｹ｡｣
ｭ｣､ﾎｹ爨ﾏﾈ賽罕ｲ･､･ｬ1､ﾎ､ﾈ､ｭ､ﾋturnﾃﾍ､ﾎﾉ､ｬ｡ﾞ100､ﾞ､ﾇ､ﾋ､ﾊ､�､隍ｦ､ﾋ､ｷ､ｿｷｸｿﾇ､ｹ｡｣
､ｳ､ﾎ･ﾗ･愠ｰ･鬣爨ﾂｹﾔ､ｷ､ﾆ､筍｢･愠ﾜ･ﾃ･ﾈ､ﾏ･ｸ･ｰ･ｶ･ｰ､ﾋﾁﾃ､ﾆ､ｷ､ﾞ､ｦ､ｫ､筅ｷ､�､ﾞ､ｻ､｣､ｷ､ｫ､ｷ｡｢ﾁｰ､ﾎ�酖ﾍ､ﾇﾀｩｸ讀ｷ､ﾆ､､､ｿ､筅ﾎ､隍熙筍｢･愠ﾜ･ﾃ･ﾈ､ﾎｿｶ､�､ﾏｴﾋ､荀ｫ､ﾋ､ﾊ､ﾃ､ﾆ､､､�､ﾏ､ｺ､ﾇ､ｹ｡｣
ﾂｮﾅﾙ､ﾈﾈ賽罕ｲ･､･ｬﾀﾚ､ﾋﾀﾟﾄ熙ｹ､�､ﾐ｡｢､ｫ､ﾊ､�ｰﾂﾄ熙ｷ､ﾆﾁ�､�､隍ｦ､ﾋ､ﾊ､熙ﾞ､ｹ｡｣
ﾈ賽罕ｲ･､･遉ｭ､ｯ､ｷ､ｹ､ｮ､�､ﾈ｡｢･愠ﾜ･ﾃ･ﾈ､ｬｶﾊ､ｬ､熙ｹ､ｮ､�､ﾎ､ﾇﾉﾔｰﾂﾄ熙ﾋ､ﾊ､熙ﾞ､ｹ｡｣
ｵﾕ､ﾋｾｮ､ｵ､ｯ､ｷ､ｹ､ｮ､�､ﾈ｡｢ｿｶ､�､ﾏｾｮ､ｵ､ｯ､ﾊ､熙ﾞ､ｹ､ｬ｡｢･ｫ｡ｼ･ﾖ､ﾊ､ﾉ､ｬｶﾊ､ｬ､熙ｭ､�､ﾊ､ｯ､ﾊ､熙ﾞ､ｹ｡｣
ﾅｬﾀﾚ､ﾊﾈ賽罕ｲ･､･ﾈﾂｮﾅﾙ､ﾏ｡｢･ｳ｡ｼ･ｹ､茹愠ﾜ･ﾃ･ﾈ､ﾋ､隍ﾃ､ﾆｰﾛ､ﾊ､�､ﾎ､ﾇ｡｢ｻ鋕ｳﾁﾔ､ｷ､ﾆﾅｬﾀﾚ､ﾊﾃﾍ､ｵ､熙ﾞ､ｷ､遉ｦ｡｣