2011-02-05

キャタピラ型再挑戦

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);

SetSensorTouch(IN_4);

#define TIME 50

int standerd=30;//変数standerdに白黒の中点を入力するこれは以後の出力の基準点となる

float POW;

int currentsensor;

int previoussensor;

float kp=1;

int ki;

float kd=0.5;

float P;

int I;

float D;

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

NumOut(60,LCD_LINE6,SENSOR_2);//ディスプレイにセンサー２の数値を表示

NumOut(60,LCD_LINE7,kp);//ディスプレイにkpの数値を表示

NumOut(60,LCD_LINE8,kd);//ディスプレイにkdの数値を表示

if(Sensor(IN_1)==1){

kp = kp + 0.1 ;

}

if(Sensor(IN_4)==1){

kd = kd + 0.1 ;

}

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = P ; }

if( D < 0 ){

D = D ; }

POW =P + D;

OnFwd(OUT_C,30 - POW);

OnFwd(OUT_A,30 + POW);

Wait(TIME);

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-02-03

カクカクの原因判明

と思ったらrotatemotorのデフォルトだとブレーキがかかるからだった。
ブレーキの反動でかくかくしていたのだ。ブレーキを掛けないためにrotetemotorでfalseと入れるとブレーキがかからないfloat型になりかくかくしなくなりました。コレについて記載したサイトは日本語だと10ページないと思う。

ポイント
角度を回転させるためにRotatemotorではでふぉるとではブレーキを使っている。
秒間10回もステアリングをふるのであれば出力は10から20程度で十分

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);

SetSensorTouch(IN_4);

#define TIME 50

int standerd;

int POW;

int currentsensor;

int previoussensor;

int steering =0;

int presteering =0;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

currentsensor=Sensor(IN_2);//現在の光量を計測する

NumOut(80,LCD_LINE4,currentsensor);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE5,kp);//ディスプレイにkpの数値を表示

NumOut(80,LCD_LINE6,kd);//ディスプレイにkdの数値を表示

NumOut(80,LCD_LINE7,steering);//ディスプレイにsteeringの数値を表示

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

steering = currentsensor-standerd;//ステアリング角は現センサー値と目標値との差

steering = steering - presteering;//今回ステアリング角度より前回ステアリング角をマイナスする。なお前回ステアリング値の初回値はゼロなので一回目のステアリングには影響しない。

if( steering < -40 ){

steering = -40 ; }

if( steering > 40 ){

steering = 40 ; }

RotateMotorEx(OUT_C,15,steering,0,false,false);//ステアリング値正で左回転ステアリング値基準値以下であれば左にかじを切るつまり右エッジ走行

presteering = steering ;

OnFwd(OUT_A,-25 );

Wait(TIME);

}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-02-03

ステアリング型　舵角戻らないタイプ

かじを切ったあとに舵角を戻らないようにセットしてみたが謎の行動爆発を起こす。
舵を切りたい角度マイナス舵を切った角度＝舵をこれから切るべき角度のはずなんだけど
くるくるくるくる舵が回る
ううーむ、ドツボ。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);

SetSensorTouch(IN_4);

#define TIME 50

int standerd;

int POW;

int currentsensor;

int previoussensor;

int steering =0;

int presteering =0;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

currentsensor=Sensor(IN_2);//現在の光量を計測する

NumOut(80,LCD_LINE1,currentsensor*5);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE4,currentsensor);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE5,kp);//ディスプレイにkpの数値を表示

NumOut(80,LCD_LINE6,kd);//ディスプレイにkdの数値を表示

NumOut(80,LCD_LINE7,steering);//ディスプレイにsteeringの数値を表示

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

steering = currentsensor-standerd;//ステアリング角は現センサー値と目標値との差

if( steering < -40 ){

steering = -40 ; }

if( steering > 40 ){

steering = 40 ; }

RotateMotor(OUT_C,100,steering);//
presteering = steering ;//出力後今回ステアリング値を過去ステアリング角として保持する。

OnFwd(OUT_A,-25 );

Wait(TIME);

}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-02-02

ライントレースロボット　ステアリング型　オンオフ制御

こちらが完成形ただし幅が２ｃｈのニチバン黒テープをあっさりまたぐ。
ETロボコンは幅が3chのラインで最小R半径は30ｃｈとかなりシビアな戦いをしている。

１単位あたりのタイムを50ミリSECに変えてもそれほど挙動は変わらないし、
このへんがNXTの限界なのか？コードが長過ぎて悪いから遅い反応しかできてないのか？
ううーむ曲がれない。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);

SetSensorTouch(IN_4);

#define TIME 50

int standerd;

int POW;

int currentsensor;

int previoussensor;

int steering =0;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

NumOut(80,LCD_LINE4,SENSOR_2);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE5,kp);//ディスプレイにkpの数値を表示

NumOut(80,LCD_LINE6,kd);//ディスプレイにkdの数値を表示

NumOut(80,LCD_LINE7,steering);//ディスプレイにsteeringの数値を表示

if(Sensor(IN_1)==1){

kp = kp + 1 ;

}

if(Sensor(IN_4)==1){

kd = kd + 1 ;

}

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

if(currentsensor<28){

steering=steering+20;//steering値更新steering値プラマイ30以上であるならそれ以上推移しない

if(steering >41 ){

steering = 40;}

RotateMotor(OUT_C,60,steering);//motorsyuturyoku

OnFwd(OUT_A,-25 );

Wait(TIME);

RotateMotor(OUT_C,60,-steering);
}

else{

steering=steering-20;//steering値更新

if(steering <-41 ){

steering = -40;}

RotateMotor(OUT_C,60,steering);

OnFwd(OUT_A,-25 );

Wait(TIME);

RotateMotor(OUT_C,60,-steering);
}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-01-29

ライントレースロボット6　ステアリング値作成

よく見てみるとステアリング値の初期値０がない。
task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);
SetSensorTouch(IN_4);

#define TIME 100

int standerd;

int POW;

int currentsensor;

int previoussensor;

int steering;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

NumOut(80,LCD_LINE4,SENSOR_2);//ディスプレイにセンサー２の数値を表示
NumOut(80,LCD_LINE5,kp);//ディスプレイにkpの数値を表示
NumOut(80,LCD_LINE6,kd);//ディスプレイにkdの数値を表示
NumOut(80,LCD_LINE7,steering);//ディスプレイにsteeringの数値を表示

if(Sensor(IN_1)==1){
kp = kp + 1 ;
}

if(Sensor(IN_4)==1){
kd = kd + 1 ;
}

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

if(currentsensor<28){

steering=steering+10;//steering値更新steering値プラマイ30以上であるならそれ以上推移しない
if(steering >31 ){
steering = 30;}
RotateMotor(OUT_C,60,steering);//motorsyuturyoku

OnFwd(OUT_A,-40 );

Wait(TIME);}

else{

steering=steering-10;//steering値更新
if(steering <-31 ){
steering = -30;}
RotateMotor(OUT_C,60,steering);

OnFwd(OUT_A,-40 );

Wait(TIME);}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-01-27

PIDライントレースロボット５ステアリング型へ変更

ある程度までPD型を考えてみたが、キャタピラ型でのPID制御に限界を感じたのでステアリング型へ変更。
キャタピラ型の問題点は回転するときに必ず曲がる方の出力を下げなければいけなくてロスしてしまうこと。
曲がったあとで自然に舵角を中心にもどすようにしたいがどのようにすればよいか不明。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);
SetSensorTouch(IN_4);

#define TIME 100

int standerd;

int POW;

int currentsensor;

int previoussensor;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

NumOut(80,LCD_LINE6,SENSOR_2);//ディスプレイにセンサー２の数値を表示
NumOut(80,LCD_LINE7,kp);//ディスプレイにkpの数値を表示
NumOut(80,LCD_LINE8,kd);//ディスプレイにkdの数値を表示

if(Sensor(IN_1)==1){
kp = kp + 1 ;
}

if(Sensor(IN_4)==1){
kd = kd + 1 ;
}

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

if(currentsensor<28){

RotateMotor(OUT_C,60,10);

OnFwd(OUT_A,-40 );

Wait(TIME);}

else{

RotateMotor(OUT_C,60,-10);

OnFwd(OUT_A,-40 );

Wait(TIME);}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}

2011-01-25

PID制御ライントレースロボット４　D制御追加

D制御文を加えて、ｋｐとｋｄをタッチセンサーを押すたびに＋１にするようにして適切なｋｐ・ｋｄを探せるようにした。
現在のｋｐ値、ｋｄ値は当然ディスプレイでモニターする。

しかしここでモータ逆転・ノッキングするようになってしまった。
ｋｐ・ｋｄどうこう以前に
D値制御部分を加えたことによりPOW値がマイナスになって逆転してしまっているようだが・・・。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);
SetSensorTouch(IN_4);

#define TIME 100

int standerd;

int POW;

int currentsensor;

int previoussensor;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

if(Sensor(IN_1)==1){
kp = kp + 1 ;
}

if(Sensor(IN_4)==1){
kd = kd + 1 ;
}

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

if(currentsensor<28){

OnFwd(OUT_C,60 + POW);
OnFwd(OUT_A,30 - POW );

Wait(TIME);}

else{

OnFwd(OUT_C,30 - POW);

OnFwd(OUT_A,60 + POW);

Wait(TIME);}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}