[[2009b/A2/ロボコン]]
-----------------------
CENTER:&size(30){ピンポン玉を集めて入れるロボットのプログラム};
-----------------------
*ピンポン玉を集めて入れるロボットNo.1 [#r1d1437a]
#define Bright 45 //閾値
#define GOFWD(t) OnRev(OUT_A+OUT_C);Wait(t); //前進
#define Stop Off(OUT_A+OUT_C); //停止
#define GOREV OnFwd(OUT_A+OUT_C); //後退
task main (){
SetSensor(SENSOR_1,SENSOR_ROTATION); //センサー1を回転センサー
SetSensor(SENSOR_2,SENSOR_LIGHT); //センサー2を光センサー(前)
SetSensor(SENSOR_3, SENSOR_LIGHT); //センサー3を光センサー(後)
Wait(3000); //30秒待つ
while(SENSOR_2>Bright){ //光センサー(前)が暗くなるまで前進
GOFWD(10);
}
Wait(10); //0.01秒前進
while(SENSOR_2>Bright){ //光センサー(前)が暗くなるまで、左に曲がる
OnRev(OUT_C);
Off(OUT_A);
}
PlaySound(SOUND_DOUBLE_BEEP);
ClearSensor(SENSOR_1); //回転センサーをリセット
while (abs(SENSOR_1)< 60) { //回転センサーの絶対値が60未満の間
if (SENSOR_2 < Bright) { //光センサー(前)が暗かったら
OnRev(OUT_A); //0.2秒、右に曲がる
Off(OUT_C);
Wait(20);
}else{ //その他の場合、0.2秒、左に曲がる
Off(OUT_A);
OnRev(OUT_C);
Wait(20);
}
}
Stop; //停止
OnRev(OUT_B); //アームを動かし始める
ClearTimer(0); //タイマー0をリセット
while(FastTimer(0)<150){ //タイマー0が1.5秒未満の間、以下を繰り返す
OnRev(OUT_A); //大きく右に曲がる(0.6秒)
Float(OUT_C);
Wait(60);
OnFwd(OUT_C); //右回りに回転(0.2秒)
Wait(20);
}
Stop; //停止
PlaySound(SOUND_DOUBLE_BEEP);
ClearSensor(SENSOR_1); //回転センサーをリセット
while(abs(SENSOR_1)<40){ //回転センサーの絶対値が40未満の間
GOFWD(10); //前進
}
Stop; //停止(1秒)
Wait(100);
PlaySound(SOUND_FAST_UP);
ClearSensor(SENSOR_1); //回転センサーをリセット
while(abs(SENSOR_1)<40){ //回転センサーの絶対値が40未満の間
GOREV; //後退
}
Stop; //停止
Off(OUT_B); //アームを止める
while(SENSOR_2>Bright){ //光センサー(前)が暗くなるまで
OnFwd(OUT_C); //右に回転
OnRev(OUT_A);
}
PlaySound(SOUND_FAST_UP);
Stop; //停止
//ここから後ろ向きに走行
ClearTimer(0); //タイマー0をリセット
while (FastTimer(0)<500) { //タイマー0が5秒未満の間、以下を繰り返す
if (SENSOR_3 < Bright) { //光センサー(後)が暗かったら
OnFwd(OUT_C); //後ろの方向で右へ曲がる
Off(OUT_A);
}else { //その他の場合
Off(OUT_C); //後ろの方向で左へ曲がる
OnFwd(OUT_A);
}
}
Stop; //停止
ClearSensor(SENSOR_1); //回転センサーをリセット
while(abs(SENSOR_1)<=20){ //回転センサーの絶対値が20以下の間、以下を繰り返す
OnFwd(OUT_C); //大きく右に曲がる(0.2秒)
Float(OUT_A);
Wait(20);
OnRev(OUT_A); //右回りに回転(0.05秒)
Wait(5);
}
ClearTimer(0); //タイマー0をリセット
while(FastTimer(0)<500){ //タイマー0が5秒未満の間
OnFwd(OUT_A+OUT_C); //後退
}
while(SENSOR_2>Bright){ //光センサー(前)が暗くなるまで
OnRev(OUT_A+OUT_C); //前進
}
//ここから前向きに走行
ClearSensor(SENSOR_1); //回転センサーをリセット
while (abs(SENSOR_1)<=450) { //回転センサーの絶対値が450以下の間
if (SENSOR_2 < Bright) { //光センサー(前)が暗かったら
OnRev(OUT_A); //右に曲がる
Off(OUT_C);
}else { //その他の場合、
Off(OUT_A); //左に曲がる
OnRev(OUT_C);
}
}
Stop; //停止
}
*ピンポン玉を集めて入れるロボットNo.2 [#nf733c40]
#define FWD_AC OnFwd(OUT_A+OUT_C); //モーターAとCを前進
#define FWD_A OnFwd(OUT_A); //モーターAを前進
#define FWD_C OnFwd(OUT_C); //モーターCを前進
#define REV_AC OnRev(OUT_A+OUT_C); //モーターAとCを後退
#define REV_A OnRev(OUT_A); //モーターAを後退
#define REV_C OnRev(OUT_C); //モーターCを後退
#define OFF_AC Off(OUT_A+OUT_C); //モーターAとCを停止
#define OFF_A Off(OUT_A); //モーターAを停止
#define OFF_C Off(OUT_C); //モーターCを停止
#define FLOAT_AC Float(OUT_A+OUT_C); //モーターAとCをFloatで停止
#define FLOAT_A Float(OUT_A); //モーターAをFloatで停止
#define FLOAT_C Float(OUT_C); //モーターCをFloatで停止
#define THRESHOLD 45 //閾値
#define ARM_OPEN OnFwd(OUT_B); //アームを開く
#define ARM_CLOSE OnRev(OUT_B); //アームを閉じる
#define ARM_STOP Off(OUT_B); //アームを止める
task main(){
SetSensor(SENSOR_1,SENSOR_LIGHT); //センサー1を光センサー(左)
SetSensor(SENSOR_2,SENSOR_ROTATION); //センサー2を回転センサー
SetSensor(SENSOR_3,SENSOR_LIGHT); //センサー3を光センサー(右)
Wait(3000); //30秒待つ
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<=21){ //回転センサーの絶対値が21以下の間
FWD_AC; //前進
}
while(abs(SENSOR_2)<=45){ //回転センサーの絶対値が45以下の間
line_trace(); //左右の光センサーを使ってライントレースをする
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<5){ //回転センサーの絶対値が5未満の間
FWD_A;REV_C; //右回りに回転
}
OFF_AC; //停止(1秒)
Wait(100);
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<25){ //回転センサーの絶対値が25未満の間
FWD_AC; //前進
}
OFF_AC; //停止(1秒)
Wait(100);
ARM_CLOSE;Wait(50); //アームを0.5秒間閉じ始める
ARM_STOP; //アームを止める
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<24){ //回転センサーの絶対値が24未満の間
REV_AC; //後退
}
OFF_AC; //停止(1秒)
Wait(100);
while(SENSOR_1>THRESHOLD) { //光センサー(左)が明るい間
FWD_C;REV_A; //左回りに回転
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(SENSOR_3>THRESHOLD){ //光センサー(右)が明るい間
line_trace1(); //光センサー(左)だけのライントレース
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<=15){ //回転センサーの絶対値が15以下の間
FWD_A;REV_C; //右回りに回転
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<30){ //回転センサーの絶対値が30未満の間
line_trace1(); //光センサー(左)だけのライントレース
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<5){ //回転センサーの絶対値が5未満の間
FWD_C;REV_A; //左回りに回転
}
OFF_AC; //停止(0.01秒)
Wait(10);
FWD_AC; //前進(3秒)
Wait(300);
OFF_AC; //停止
ARM_OPEN;Wait(30); //アームを開く(0.03秒)
ARM_STOP; //アームを止める
while((SENSOR_1>THRESHOLD) || (SENSOR_3>THRESHOLD)){ //光センサー(左)と光センサー(右)が共に暗くなるまで
REV_AC; //後退
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<=5){ //回転センサーの絶対値が5以下の間
FWD_C;REV_A; //左回りに回転
}
while(SENSOR_1>THRESHOLD){ //光センサー(左)が暗くなるまで
line_trace3(); //光センサー(右)だけのライントレース
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<=5){ //回転センサーの絶対値が5以下の間
FWD_C;REV_A; //左回りに回転
}
ClearSensor(SENSOR_2); //回転センサーをリセット
while(abs(SENSOR_2)<200){ //回転センサーの絶対値が200未満の間
line_trace3(); //光センサー(右)だけのライントレース
}
OFF_AC; //停止
}
sub line_trace(){ //左右の光センサーを用いたライントレース
if(SENSOR_1<THRESHOLD){ //光センサー(左)が暗かったら
FWD_C;REV_A; //左回りに回転
}else if(SENSOR_3<THRESHOLD){ //光センサー(右)が暗かったら
FWD_A;REV_C; //右回りに回転
}else { //その他の場合
FWD_AC; //前進
}
}
sub line_trace1(){ //左の光センサーだけを用いたライントレース
if(SENSOR_1<THRESHOLD){ //光センサー(左)が暗かったら
FWD_A;OFF_C; //右に曲がる
}else{ //その他の場合
FWD_C;OFF_A; //左に曲がる
}
}
sub line_trace3(){ //右の光センサーだけを用いたライントレース
if(SENSOR_3<THRESHOLD){ //光センサー(右)が暗かったら
FWD_C;OFF_A; //左に曲がる
}else{ //その他の場合
FWD_A;OFF_C; //右に曲がる
}
}
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
