![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
超音波センサーを、一番前につけて、少し上下できるように取り付けた。周囲の空き缶を見つけ出して、ボールをあてるということで、アームをできるだけ高い位置の取り付けて、位置エネルギーを利用して、ボールをぶつけるという方式をとった。また、タイヤの位置はできるだけコンパクトに収まるように、本体の下に直接くるように、LEGOで台を作って、そこに取り付けた。また、ロボットをしっかりと固定するために、たくさん、固定のための部品をつなぎ目の部分に取り付けた。
#define ObstacleDistance 25 //対象物体との距離 #define DetectWaitTime 1000 //超音波センサーの感覚 const float diameter = 5.45; //タイヤの直径 const float pi = 3.141515; //円周率 const float track = diameter*2.0; //微調整したトラック幅 #define speed_slow 30 //speedの設定 int Sum_Counter = 0; //超音波センサーが測定した合計回数
float FwdisRo(float d){
float ang = d/(diameter*pi)*360.0;
return ang;
}
タイヤの直径から、進みたい距離を回転数に変換する。
void turnAng(long ang){
long angle; angle = (track/diameter)*ang;
RotateMotorEx(OUT_AB,-speed_slow,angle,100,true,true);
}
旋回角度をトラック幅とタイヤの直径から割出し、RotateMotor関数で、反転同期させて旋回する。
inline void OnDisplaySonic(){
ClearScreen();
TextOut(0,LCD_LINE1,"UltraSensor:");
NumOut(80,LCD_LINE1,SensorUS(S4));
TextOut(0,LCD_LINE2,"Distance");
NumOut(80,LCD_LINE2,FwdisRo(SensorUS(S4)-ObstacleDistance));
}
超音波センサーの値と、物体との距離をディスプレイに表示する。
inline void OnDisplaySonicSHU(){
TextOut(0,LCD_LINE3,"MotorA:");
NumOut(80,LCD_LINE3,MotorTachoCount(OUT_A));
TextOut(0,LCD_LINE4,"MotorB:");
NumOut(80,LCD_LINE4,MotorTachoCount(OUT_B));
}
左右のモーターの回転数をディスプレイに表示する。
inline void Back_Or_Forward(int pwr){
float angle = FwdisRo(SensorUS(S4)-ObstacleDistance);
OnDisplaySonic();
RotateMotor(OUT_AB,pwr,-angle);
OnDisplaySonicSHU();
Wait(DetectWaitTime);
Float(OUT_AB);
}
進みたい距離を超音波センサーの値との誤差で得る、プラス、マイナスの値がでるので、それをそのままRotetaMotor関数し代入し、前後に距離を調整する。
task main(){
SetSensorLowspeed(S4);
while(SensorUS(S4) >61){
OnFwd(OUT_A,50);
OnRev(OUT_B,50);
}
Off(OUT_AB);
61cmよりも超音波センサーの値が高いときは旋回を続ける。範囲内に対象物体を見つけたときに停止する。
while(Sum_Counter <= 5 && SensorUS(S4) != ObstacleDistance && SensorUS(S4) < 61){
if(SensorUS(S4)<ObstacleDistance){
Back_Or_Forward(50);
Sum_Counter++;
}
if(SensorUS(S4)>ObstacleDistance){
Back_Or_Forward(50);
Sum_Counter++;
}
if(Sum_Counter > 4){
RotateMotor(OUT_C,30,100);
Float(OUT_C);
}
if(SensorUS(S4) == 25){
RotateMotor(OUT_C,30,100);
Float(OUT_C);
}
}//while end
}//task main end
ロボットが1mの範囲内に対象の物体を見つけた場合はそこで停止して、その対象との距離を25cmに保とうする、もしも、25cmときっちり同じ距離、もしくわ、調整回数が5回以上になったとき、ボールが転がされて、対象物体にヒットする。
始めは対象物体を見つける部分と、距離を調整して、ボールを離す部分とが混在してしまって、その部分をうまく機能させるために、inline関数を使用してみたり、ifの使用回数をあえて増やして、場合分けがうまくいくようにした。Countの設定ははじめはなかったが、25cmの範囲にぴったり合わせるのは時間がかかったので、ある程度の誤差を承知の上で、超音波センサーの判定が繰り返されたら、ballを離すように設定した。
ロボットを組み立てているうちに、台を取り付けて、その上に本体を乗せるほうが安定するということに気がついたり、高いところから、ボールを転がすという部分の構成を思いついた。今回はプログラムにせよ、うまいことロボットが扱えたと思います。もしも、失敗したときに、なにかしら、カバーできるようにすることが大切だと痛感した。そのためには、やたらと時間がかかってしまった場合は別の動作をするなど、一定動作が時間的もしくわ、回数的に繰り返されたときに条件分岐を考えるという方針が有効的のものだと考えるに至った。
SHU.jpg
SHO.jpg
RA.jpg
GYA.jpg
GU.jpg
