マトリョーシカ的日常

ワクワクばらまく明日のブログ。

ライントレースロボ続き。

 コード:

    • on/off制御

 

#define THRESHOLD 35

task main()
{
SetSensorColorRed(IN_2);
OnFwd(OUT_AC,50);
while(true)
{

if(Sensor(IN_2) > THRESHOLD)
{
OnRev(OUT_C,50);
Wait(100);
until(Sensor(IN_2) <= THRESHOLD);

}
NumOut(80,LCD_LINE6,SENSOR_2);
OnFwd(OUT_AC,50);
}
}

    • P 制御

#define TIME 5

task main()
{
int P;
int standerd=30;
int nowsen;
int kp=1.1;
int POWA;
int POWC;
SetSensorColorRed(IN_2);
OnFwd(OUT_AC,40);
while(true)
{
nowsen=Sensor(IN_2);
NumOut(10,LCD_LINE1,SENSOR_2);
NumOut(10,LCD_LINE2,nowsen);
NumOut(10,LCD_LINE3,standerd);
NumOut(30,LCD_LINE5,P);
NumOut(80,LCD_LINE4,POWA);
NumOut(80,LCD_LINE5,POWC);


//----caliculation----

P=(nowsen-standerd)*kp;
if (P < 0){
P = 0;
}
if (nowsen < standerd ){
POWA = 80 + P;
POWC = 50 - P;

} else{
POWA = 50 - P*P/2.5 + 10;
POWC = 50 + P;
}
if (POWA < 0){
POWA = 0;
}
OnFwd(OUT_A,POWA);
OnFwd(OUT_C,POWC);
Wait(TIME);
}
}


こんなかんじです。Pは簡単な比例制御ではうまくいきませんでした。