ﾁｰｾﾏ､ﾇ､ﾏ｡｢･愠ﾜ･ﾃ･ﾈ､ﾋﾈ賽翅ｩｸ讀ﾂﾁｷ､ﾞ､ｷ､ｿ｡｣P･ｲ･､･ｮ､ｵ､皃ﾋﾀﾟﾄ熙ｹ､�､ﾐｰﾂﾄ熙ｷ､ｿﾁ熙ﾋ､ﾊ､熙ﾞ､ｹ､ｬ｡｢ｵﾞ､ﾊ･ｫ｡ｼ･ﾖ､ｬｶﾊ､ｬ､�､ﾊ､ｯ､ﾊ､熙ﾞ､ｹ｡｣ﾂ遉ｭ､皃ﾋ､ｹ､�､ﾈ｡｢･ｫ｡ｼ･ﾖ､ﾏｶﾊ､ｬ､�､ﾞ､ｹ､ｬ｡｢･愠ﾜ･ﾃ･ﾈ､ｬｿｶ､鬢�､ﾆ､ｷ､ﾞ､､､ﾞ､ｹ｡｣
､ｳ､ﾎｾﾏ､ﾇ､ﾏ｡｢ﾁｰｾﾏ､ﾎ･ﾗ･愠ｰ･鬣爨ﾎﾉ､ｷ｡｢､ｵ､鬢ﾋｰﾂﾄ熙ｷ､ｿﾁ熙ﾜｻﾘ､ｷ､ﾞ､ｹ｡｣
･愠ﾜ･ﾃ･ﾈ､ﾏ､ｳ､�､ﾞ､ﾇ､ﾎ､筅ﾎ､妤ｭﾂｳ､ｭｻﾈ､､､ﾞ､ｹ｡｣
1｡･ﾈｬﾀｩｸ�
ﾈｬﾀｩｸ譯ﾊDﾀｩｸ譯ﾋ､ﾏ｡｢ｵﾞｷ网ﾊｽﾐﾎﾏﾃﾍ､ﾎﾊﾑｲｽ､ｬｵｯ､ｳ､ﾃ､ｿｾ�ｹ遑｢､ｽ､ﾎﾊﾑｲｽ､ﾎﾂ遉ｭ､ｵ､ﾋﾈ賽网ｷ､ｿﾆﾎﾏ､ﾔ､ｦ､ｳ､ﾈ､ﾇ｡｢､ｽ､ﾎﾊﾑｲｽ､ﾋｹｳ､ｷ､隍ｦ､ﾈ､ｹ､�ﾌﾜ､ﾌ､ｿ､ｷ､ﾞ､ｹ｡｣､ﾞ､ｿ｡｢･ﾏ･ﾁ･ｰ｡ﾊｼｫﾎ蠻ｪ､ﾊｿｶﾆｰ｡ﾋ､ﾞ､ｨ､�ﾆｯ､ｭ､ｷ､ﾞ､ｹ｡｣
ﾊﾑｲｽ､ﾎﾂ遉ｭ､ｵ｡ﾊﾊﾐｺｹ､ﾎﾈｬ｡ﾋ､ﾋﾈ賽网ｷ､ﾆｽﾐﾎﾏ､ﾑｲｽ､ｵ､ｻ､�､ｿ､癸｢ｵﾞ､ﾋ･ｫ｡ｼ･ﾖ､ﾋｺｹ､ｷｳﾝ､ｫ､ﾃ､ｿｻ､茖｢･愠ﾜ･ﾃ･ﾈ､ｬﾂ遉ｭ､ｯｿｶ､�､ﾆ､､､�､ﾈ､ｭ､ﾏﾀ軏ﾍ､ｬﾂ遉ｭ､ｯ､ﾊ､熙ﾞ､ｹ｡｣
ｵﾕ､ﾋ｡｢ﾄｾﾀ､ﾃ､ﾆ､､､�､ﾈ､ｭ､ﾊ､ﾉ｡｢･愠ﾜ･ﾃ･ﾈ､ｬｰﾂﾄ熙ｷ､ﾆ､､､�､ﾈ､ｭ､ﾏ｡｢ﾀ軏ﾍ､ﾏｾｮ､ｵ､ｯ､ﾊ､熙ﾞ､ｹ｡｣(ｿﾞ)
･愠ﾜ･ﾃ･ﾈ､ﾎ･鬣､･ﾈ･�｡ｼ･ｹ･ﾗ･愠ｰ･鬣爨ﾋ｡｢ﾁｰｾﾏ､ﾎﾈ賽翅ｩｸ讀ﾋｲﾃ､ｨ｡｢ﾈｬﾀｩｸ讀ｳﾆ､ｷ､ﾞ､ｹ｡｣
ｼ｡､ﾎ･ﾗ･愠ｰ･鬣爨XT､ﾋ･ﾀ･ｦ･悅ｼ･ﾉ､ｷ､ﾆ､ｯ､ﾀ､ｵ､､｡｣
ﾊﾑｲｽ､ﾎﾂ遉ｭ､ｵ｡ﾊﾊﾐｺｹ､ﾎﾈｬ｡ﾋ､ﾋﾈ賽网ｷ､ﾆｽﾐﾎﾏ､ﾑｲｽ､ｵ､ｻ､�､ｿ､癸｢ｵﾞ､ﾋ･ｫ｡ｼ･ﾖ､ﾋｺｹ､ｷｳﾝ､ｫ､ﾃ､ｿｻ､茖｢･愠ﾜ･ﾃ･ﾈ､ｬﾂ遉ｭ､ｯｿｶ､�､ﾆ､､､�､ﾈ､ｭ､ﾏﾀ軏ﾍ､ｬﾂ遉ｭ､ｯ､ﾊ､熙ﾞ､ｹ｡｣
ｵﾕ､ﾋ｡｢ﾄｾﾀ､ﾃ､ﾆ､､､�､ﾈ､ｭ､ﾊ､ﾉ｡｢･愠ﾜ･ﾃ･ﾈ､ｬｰﾂﾄ熙ｷ､ﾆ､､､�､ﾈ､ｭ､ﾏ｡｢ﾀ軏ﾍ､ﾏｾｮ､ｵ､ｯ､ﾊ､熙ﾞ､ｹ｡｣(ｿﾞ)
･愠ﾜ･ﾃ･ﾈ､ﾎ･鬣､･ﾈ･�｡ｼ･ｹ･ﾗ･愠ｰ･鬣爨ﾋ｡｢ﾁｰｾﾏ､ﾎﾈ賽翅ｩｸ讀ﾋｲﾃ､ｨ｡｢ﾈｬﾀｩｸ讀ｳﾆ､ｷ､ﾞ､ｹ｡｣
ｼ｡､ﾎ･ﾗ･愠ｰ･鬣爨XT､ﾋ･ﾀ･ｦ･悅ｼ･ﾉ､ｷ､ﾆ､ｯ､ﾀ､ｵ､､｡｣
trace_pd.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,d_gain=1.0;
int black,white,center,light,light_tmp; /* ｰ�､ﾄﾁｰ､ﾎﾌﾀ､�､ｵ､ﾊﾇｼ､ｹ､�"light_tmp"ﾊﾑｿ�ｵﾁ */
U32 timer;
float turn;
while(1){
calibration(&black,&white,¢er); /* �酖ﾍﾀﾟﾄ�ﾍﾑｴﾘｿﾆ､ﾓｽﾐ､ｷ */
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* ､筅ｷｲ｡､ｷ､ｿ､ﾈ､ｭ･ｹ･ｿ｡ｼ･ﾈ */
display_clear(0);
display_goto_xy(0, 1);
display_string("PUSH START");
display_update();
systick_wait_ms(10);
}
systick_wait_ms(1000); /* 1ﾉﾃﾂﾔ､ﾄ */
light_tmp = ecrobot_get_light_sensor(PORT_LIGHT);
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){
light = ecrobot_get_light_sensor(PORT_LIGHT);
turn = p_gain * (light - center)*100 / (black-white) + d_gain * (light-light_tmp)/(black-white)*100;
light_tmp=light; /* ｰ�､ﾄﾁｰ､ﾎﾌﾀ､�､ｵ､ﾊﾇｼ */
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･�｡ｼ･ﾗ､ﾋ､ｫ､ｫ､ﾃ､ｿｻｴﾖ､ﾇｳ荀�､ﾙ､ｭ､ﾇ､ｹ､ｬ｡｢ﾈｬ･ｲ･､･ﾋｴﾞ､皃ｿ､ｳ､ﾈ､ﾋ､ｷ､ﾆ､､､ﾞ､ｹ｡｣
ｺﾇﾅｬ､ﾊﾂｮﾅﾙ｡｢ﾈ賽罕ｲ･､･｢ﾈｬ･ｲ･､･ﾏ｡｢･ｳ｡ｼ･ｹ､茹愠ﾜ･ﾃ･ﾈ､ﾋ､隍ﾃ､ﾆｰﾛ､ﾊ､熙ﾞ､ｹ｡｣
､ｿ､ﾀ､ｷ｡｢ﾈ賽罕ｲ･､･ﾏ｡｢ﾁｰｾﾏ､ﾎ･ﾗ･愠ｰ･鬣爨隍熙篝ｮ､ｵ､ｯ､ｷ､ｿ､ﾛ､ｦ､ｬﾎﾉ､､､ﾇ､ｷ､遉ｦ｡｣
ﾁｰｾﾏ､ﾎﾈ賽翅ｩｸ讀ﾋｲﾃ､ｨ｡｢ﾈｬﾀｩｸ讀ﾂﾁｹ､�､ﾈ｡｢ｰﾂﾄ熙ｷ､ｿ･鬣､･ﾈ･�｡ｼ･ｹ､ｬｽﾐﾍ隍�､隍ｦ､ﾋ､ﾊ､�､ｳ､ﾈ､ｬ､�､ｫ､熙ﾞ､ｹ｡｣