コード:
-
- 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は簡単な比例制御ではうまくいきませんでした。