2015b/Member/rmsun/Mission3
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2015b/Member]]
目次
#contents
*ロボコンの説明 [#p90c333d]
#ref(2015b/Member/rmsun/Mission3/2015b-mission31.png, 480...
ロボットAはSTART A地点から出発し、P地点におかれた赤いボー...
詳しい説明はhttp://yakushi.shinshu-u.ac.jp/robotics/?2015...
*ロボットの説明 [#p06dabf5]
#ref(2015b/Member/rmsun/Mission3/IMG_1411.JPG, 480*360);
写真左:ボールを缶に当てるロボット
写真右:ボールを回収するロボット
**「ボールを缶に当てるロボット」 [#k9cf5b32]
***ロボットの構造 [#i789b231]
&ref(2015b/Member/rmsun/Mission3/IMG_1410a.JPG,360*480);
&ref(2015b/Member/rmsun/Mission3/IMG_1409a.JPG,480*360);
-モータAはアームの部分、モータBは右タイヤ、モータCは左タ...
-使ったセンサは、光センサと超音波センサ。
-超音波センサは上下に動く。
-アームの動きと超音波センサの向きが連動している(例 : アー...
***ロボットの動き [#q2a47db6]
?超音波センサを下向きにして、ボールを探す(ライントレース...
↓↓
?ボールを見つけるとアームを閉じ、超音波センサは正面向きに
↓↓
?超音波センサで缶を探す
↓↓
?缶の方を向く
↓↓
?ボールを放つ
-ボールの放ち方
#ref(2015b/Member/rmsun/Mission3/ロボコン6.gif,480*332)
**「ボールを回収するロボット」 [#l707841a]
***ロボットの構造 [#y585dbcb]
#ref(2015b/Member/rmsun/Mission3/IMG_1406a.JPG,360*480)
&ref(2015b/Member/rmsun/Mission3/IMG_1407a.JPG,240*180);
&ref(2015b/Member/rmsun/Mission3/IMG_1408a.JPG,180*240);
-モータAはアーム部分、モータBは右タイヤ、モータCは左タイ...
-使ったセンサは、光センサと超音波センサ。
-超音波センサは、(センサの位置の関係もあり)対象との距離を...
***ロボットの動き [#mb50a946]
缶←←ボール
↑
↑超音波センサ
ロボット
上図のように超音波センサを向ける。
ボールが通り過ぎた時 : 超音波センサはボールを感知→0.8秒停...
1. ボールが跳ね返ったとき
缶→→ボール
↑
↑超音波センサ
ロボット
超音波センサで再び感知→ロボット直進→ボールをつかむ→ライン...
2. ボールが左に流れてしまったとき
2.1 跳ね返ったボールを感知しない
ボール←←
缶
↑
↑超音波センサ
ロボット
超音波センサはボールを感知しない→ロボット左へ向かう
2.2 缶の左側に向かい、ボールを探索
ボール
↑↑↑↑↑↑ 缶
↑↑↑↑
↑↑ 矢印は、超音波センサが作用する範囲
ロボット
☆缶を感知しない程度に、ロボット(超音波センサ)を回転させる。
-ボールが見つかった場合
ボールの方を向き、直進。その後ボールをつかむ
-ボールが見つからなかった場合
ロボットを下図の向きに戻して、少し進んで、再び☆の動作
缶
↑
↑ 矢印の方へ向く
ロボット
*「ボールを缶に当てるロボット」のプログラム [#k14bab2c]
自分の書いたプログラムではないので、詳しく説明はできませ...
#define straight OnFwd(OUT_BC,30);
#define turn_left OnFwd(OUT_B,30);Off(OUT_C);
#define turn_right OnFwd(OUT_C,30);Off(OUT_B);
#define TURN_LEFT OnFwd(OUT_B,30);OnRev(OUT_C,30);
#define TURN_RIGHT OnFwd(OUT_C,30);OnRev(OUT_B,30);
#define THRESHOLD 35
#define get OnFwd(OUT_A,45);Wait(300);Off(OUT_A);Wait(10...
#define open OnRev(OUT_A,45);Wait(300);Off(OUT_A);Wait(1...
const float diameter=5.45;
const float track=10.35;
const float pi=3.1415;
void fwdDist(float d) { //dcm進む
long angle;
angle=d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,30,angle,0,true,true);
}
void turnAng(long ang) { //ang度右旋回
long angle;
angle=track/diameter*ang;
RotateMotorEx(OUT_BC,30,angle,100,true,true);
}
int searchDirection(long ang) { //最短距離の物体の方を向く
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=55;
angle=(track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,30,-100);
while(MotorTachoCount(OUT_B)<=angle){
if(SensorUS(S4)<d_min){
PlaySound(SOUND_CLICK);
d_min=SensorUS(S4);
tacho_min=MotorTachoCount(OUT_B);
}
}
OnFwdSyncEx(OUT_BC,30,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S4)<=...
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;
}
task main() {
SetSensorLight(S3);
SetSensorLowspeed(S4);
int catch=0;
straight;
Wait(500);
RotateMotor(OUT_A,30,-80);
while(true){
if(SensorUS(S4)>=12){ //超音波センサ値 >= 12 つまり、...
if(SENSOR_3<THRESHOLD-9){
TURN_LEFT;
}else if(SENSOR_3<THRESHOLD-5){
turn_left;
}else if(SENSOR_3<THRESHOLD+5){
straight;
}else if(SENSOR_3<THRESHOLD+9){
turn_right;
}else{
TURN_RIGHT;
}
}else{ //センサがボールを感知したとき
Off(OUT_BC);
get;
Wait(1000);
catch++;
if(catch==1){
RotateMotorEx(OUT_BC,40,-200,0,true,false);
RotateMotorEx(OUT_BC,40,200,-100,true,false);
Off(OUT_BC);
Wait(500);
RotateMotorEx(OUT_BC,40,700,0,true,false);
Off(OUT_BC);
Wait(500);
int d=searchDirection(130);
PlaySound(SOUND_UP);
if(d<=50){ //超音波センサが缶を感知した時した時
Off(OUT_BC); //↓ここからボールを放つ動作
Wait(500);
RotateMotorEx(OUT_BC,40,-300,0,true,false); //下がる
Wait(6000);
open; //アームを開く
RotateMotorEx(OUT_BC,90,340,0,true,false); ...
Off(OUT_BC); //急停止、このときボールを放つ
Wait(1000);
RotateMotorEx(OUT_BC,65,-730,0,true,true); /...
RotateMotorEx(OUT_BC,40,150,100,true,false);...
RotateMotor(OUT_A,60,50); //アームを閉じる
straight; //少しの直進
Wait(2500);
}
}
}
}
}
*「ボールを回収するロボット」のプログラム [#z16e8d55]
自分が担当したのは、主にこの部分です。
**マクロ [#m15c1f4b]
#define SPEED 50 //Distなどのモータのパワーに、幅広く使用
#define SPEED_SLOW 30 //関数hand,turnAngなどで使用
#define THRESHOLD 45 //閾値
#define STEP 1 //Waitの引数として使う
#define STOP 3 //「ボールがセンサの前を横切った」かどう...
**関数 [#i3681e58]
***hand...ボールをつかむ [#y627f394]
sub hand(int ang, int plmi) { //ボールをつかむ動作
/*plmi = 1 → アーム開
plmi = -1 → アーム閉 */
RotateMotor(OUT_A, SPEED_SLOW * plmi, ang);
}
***Dist...指定した距離進む [#gc626d6c]
const float diameter=5.45; //タイヤの直径(cm)
const float pi=3.1415; //円周率
const float track = 10.35; //タイヤのトレッド幅
sub Dist(float d, int fwd_or_rev){ //dcm前転(後転)
/*fwd_or_rev = 1 → 前転
fwd_or_rev = -1 → 後転 */
long angle;
angle = d / (diameter * pi) * 360.0;
RotateMotorEx(OUT_BC, SPEED * fwd_or_rev, angle, 0, tru...
Wait(100);
}
***turnAng...指定した角度旋回する [#l31c83ee]
sub turnAng(int ang, int R_or_L) //ang度旋回
{
/* R_or_L = 1 → 時計回り
R_or_L = -1 → 半時計回り */
long angle;
angle = track / diameter * ang;
RotateMotorEx(OUT_BC, SPEED_SLOW , angle, 100* R_or_L, ...
Wait(100);
}
***searchDirection...最短距離の方を向く [#wd8a8862]
int searchDirection(int ang) //ボールを探す関数
{
SetSensorLowspeed(S1);
long angle, tacho_min = 0, tacho_corr;
int d_min;
d_min= 300; //仮の最小値
angle = (track / diameter) * ang;
turnAng(10, 1); //10度右旋回(缶の誤認を防ぐため、10度...
ResetTachoCount(OUT_B);
OnFwdSync(OUT_BC, SPEED, -100);
while(MotorTachoCount(OUT_B) <= angle) { //angleだけ左...
if (SensorUS(S1) < d_min) {
d_min = SensorUS(S1);
tacho_min = MotorTachoCount(OUT_B);
}
}
if (d_min < 30) { //「超音波センサの示した最小値」が30...
OnFwdSyncEx(OUT_BC,SPEED, 100, RESET_NONE); //最小値...
until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(...
}
else { //30以上のとき
turnAng(ang - 10, 1); //ang-10だけ右旋回(関数開始前と...
}
Off(OUT_BC); Wait(500); //0.5秒間停止
return d_min; //「超音波センサの示した最小値」を返す
}
***ライントレースする [#l58de8e2]
関数「trace」で使う関数
sub turn_left(int speedA, int speedB) { //左に曲がる
OnFwd(OUT_B,speedA); //引数speedAのパワーで、モータ...
OnRev(OUT_C,speedB); //引数speedBのパワーで、モータ...
}
sub turn_right(int speedA, int speedB) { //右に曲がる
OnFwd(OUT_C,speedB); //引数speedBのパワーで、モータ...
OnRev(OUT_B,speedA); //引数speedAのパワーで、モータ...
}
sub go_forward(int speed3) { //まっすぐ進む
OnFwd(OUT_BC,speed3); //speed3のパワーで、モータBCを...
}
関数trace...「time秒以内、かつ、TachoCount < target」の間...
1.交差点かどうかを見極める際に、タイマーではなくタイヤの...
例えば、左旋回した時(光センサ値 < THRESHOLD - 15)のタイヤ...
2.「ライントレースを終了する時間」を設定できる
ライントレースを続ける時間を、引数timeで与えられる。1の方...
sub trace(int time, long target, int speed3) {
SetSensorLight(S2);
int speed1 = 25; //モータのパワー用
int speed2 = 20; //モータのパワー用(speed1より少し...
long tacho = 0; //「その場で左旋回する」動作をし...
long t; //タイマーを使うときに使用
t = CurrentTick(); //ここで現在時刻をtに代入(CurrentTi...
while (CurrentTick() - t < time && tacho < target) {
if (SENSOR_2 < THRESHOLD -15){ //光センサの数...
turn_left(speed1, speed1); //左右のモータをspeed1...
} else if (SENSOR_2 < THRESHOLD -7) { //光センサの数...
tacho = 0; //tacho(turn_left1し続けた時の、タイヤ...
turn_left(speed1, speed2); //右モータをspeed1、左...
} else if (SENSOR_2 < THRESHOLD +7){ //光センサの数値...
tacho = 0; //tachoを0にする
go_forward(speed3); //左右のモータをspeed3にして...
} else if (SENSOR_2 < THRESHOLD +15){ //光センサの数...
tacho = 0; /*tachoを0にする*/
turn_right(speed1, speed2); /*右モータをspeed2、左...
} else { /*光センサの数値が、THRESHOLD+15より高い...
tacho = 0; /*tachoを0にする*/
turn_right(speed1, speed1); /*左右のモータをspeed1...
}
Wait(STEP); //STEP(0.001)秒だけ動く
if (SENSOR_2 >= THRESHOLD-15) { /*光センサの数値が、...
ResetTachoCount(OUT_B); /*モータの回転数をリセット...
}
tacho += MotorTachoCount(OUT_B); /*tachoに、モータの...
}
Off(OUT_BC); Wait(STEP);
PlaySound(SOUND_CLICK);
}
**メイン関数 [#z16c056e]
***実際の動作 [#ac6032c0]
プログラムの動きを画像で示した。(/*(1)*/の画像は、/*(1)*/...
/*--------(1)----------*/
#ref(2015b/Member/rmsun/Mission3/ロボコン1.gif,480*332)
/*--------(2)---------*/
#ref(2015b/Member/rmsun/Mission3/2015b-missiona.jpg,480*3...
/*--------(3)---------*/
#ref(2015b/Member/rmsun/Mission3/ロボコン3.gif,480*332)
/*--------(4)----------*/(←この動作は、改善の余地あり)
#ref(2015b/Member/rmsun/Mission3/ロボコン4.gif,480*332)
***mainのプログラム [#y460ebf3]
task main() {
/*-------------(1)--------------*/
long t;
Wait(30000); //30秒待機
Dist(48, 1); //48cm前進
turnAng(91, -1); //90度左旋回
SetSensorLowspeed(S1); //超音波センサ起動
int ball = 0; //変数「ball」...超音波センサの値 < 30 ...
while (1) { //ボールが通過する様子を調べる
if (SensorUS(S1) < 30) { /*センサ値 < 30(ボールがセン...
ball++;
}
if (ball == STOP) { //通過した回数がSTOP(=3)になったら
break; //ループを抜け出す
}
Wait(STEP); //STEP(0.001)秒間
}
Wait(800); //0.8秒間待機
PlaySound(SOUND_CLICK); //次の動作に入ることを知らせる
t = CurrentTick();
while (CurrentTick() - t < 2000) { //2秒の間繰り返す
if (SensorUS(S1) < 30) { //センサ値 < 30のとき
ball++; //ball + 1 ..つまりball = STOP + 1
break; //ループを抜け出す
}
Wait(STEP);
}
/*------------(2)----------------*/
if (ball == STOP + 1) { //超音波センサ < 30を示すまで...
OnFwd(OUT_BC, SPEED_SLOW); //SPEED_SLOW(=30)のパワー...
while (ball < STOP + 2) {
if (SensorUS(S1) < 30) {
ball++;
}
}
Off(OUT_BC); //1秒停止
Wait(1);
Dist(35, 1); //35cm前進
hand(70, -1); //ボールをつかむ
/*-----(黄線)-----*/
turnAng(30, -1); //30度右旋回(ライントレースの黒線に...
Dist(8, 1); //8cm前進(ライントレースの黒線に入るため)
/*-----(緑線)-----*/
trace(20000, 15, 40); /*「20秒以内 && TachoCount < 15...
/*-----(水色線)---*/
trace(7000, 1000, 50); /*「7秒以内 && TachoCount < 10...
/*------(青線)----*/
trace(15000, 10, 40); /*「15秒以内 && TachoCount < 10...
/*------(紫線)----*/
turnAng(75, 1); //75度右旋回
Dist(15,1); //15cm前進
/*------(赤線)-----*/
turnAng(20, 1); //20度右旋回
SetSensorLight(S2); //光センサー起動
OnFwd(OUT_BC, SPEED); //黒線までSPEED(=50)のパワーで...
while (SENSOR_2 > THRESHOLD - 15) {}
Off(OUT_BC); //停止
hand(70, 1); //ボールを離す
}
/*-------------(3)------------*/
else {
turnAng(90, -1); //90度左旋回
Dist(40, 1); //40cm進む
turnAng(90, 1); //90度右旋回
Off(OUT_BC); Wait(STEP); //0.001秒間停止
while (1) {
int d = searchDirection(60); //60度の範囲でボールを...
if (d < 30) { /*searchDirection(60)の返り値 < 30のと...
Dist(20, 1); //20cm進む
hand(70, -1); //ボールをつかむ
break; //ループから抜け出す
}
else {
OnFwd(OUT_BC, SPEED); //0.5秒進む
Wait(500);
searchDirection(0); /*searchDirectionを2連続で使う...
}
}
/*------------(4)---------------*/
trace(10000, 10000, 45); //どこかのラインにのる,ロボッ...
Dist(40, 1); //40cm進む、
SetSensorLight(S2); //光センサー起動
OnFwd(OUT_BC, SPEED); //SPEED(=50)のパワーで、黒線に到...
while (SENSOR_2 > THRESHOLD - 15) {}
trace(3000, 10000, 40); //「3秒以内 && TachoCount < 10...
hand(70, 1); //ボールを離す
}
}
*コメント[#od03dfdc]
**工夫点 [#oe6a4c64]
-ボールが缶に当たった後の「ボールが跳ね返った場合」と「ボ...
-「ボールがあったらボールの方向を向き、ボールがなかったら...
**反省点 [#b3dc3c0c]
-/*(4)*/の部分が不安定。ボールの位置によって、ゴールに到...
-超音波センサに頼りすぎた。超音波センサが予期せぬ動作をし...
-缶より手前(ロボット側)にボールが飛んできたとき、缶に当た...
-「二個目のボールを探すためのプログラム」が書けなかった(...
-投げる側のロボットを超音波センサが誤認するときがあった。
-投げる側のロボットと衝突するときがあった。
**感想 [#q20ee53a]
-ハードウェアの性能によって、ロボットのできることが大きく...
-超音波センサがうまく機能しないとき、ロボットの誤作動を防...
終了行:
[[2015b/Member]]
目次
#contents
*ロボコンの説明 [#p90c333d]
#ref(2015b/Member/rmsun/Mission3/2015b-mission31.png, 480...
ロボットAはSTART A地点から出発し、P地点におかれた赤いボー...
詳しい説明はhttp://yakushi.shinshu-u.ac.jp/robotics/?2015...
*ロボットの説明 [#p06dabf5]
#ref(2015b/Member/rmsun/Mission3/IMG_1411.JPG, 480*360);
写真左:ボールを缶に当てるロボット
写真右:ボールを回収するロボット
**「ボールを缶に当てるロボット」 [#k9cf5b32]
***ロボットの構造 [#i789b231]
&ref(2015b/Member/rmsun/Mission3/IMG_1410a.JPG,360*480);
&ref(2015b/Member/rmsun/Mission3/IMG_1409a.JPG,480*360);
-モータAはアームの部分、モータBは右タイヤ、モータCは左タ...
-使ったセンサは、光センサと超音波センサ。
-超音波センサは上下に動く。
-アームの動きと超音波センサの向きが連動している(例 : アー...
***ロボットの動き [#q2a47db6]
?超音波センサを下向きにして、ボールを探す(ライントレース...
↓↓
?ボールを見つけるとアームを閉じ、超音波センサは正面向きに
↓↓
?超音波センサで缶を探す
↓↓
?缶の方を向く
↓↓
?ボールを放つ
-ボールの放ち方
#ref(2015b/Member/rmsun/Mission3/ロボコン6.gif,480*332)
**「ボールを回収するロボット」 [#l707841a]
***ロボットの構造 [#y585dbcb]
#ref(2015b/Member/rmsun/Mission3/IMG_1406a.JPG,360*480)
&ref(2015b/Member/rmsun/Mission3/IMG_1407a.JPG,240*180);
&ref(2015b/Member/rmsun/Mission3/IMG_1408a.JPG,180*240);
-モータAはアーム部分、モータBは右タイヤ、モータCは左タイ...
-使ったセンサは、光センサと超音波センサ。
-超音波センサは、(センサの位置の関係もあり)対象との距離を...
***ロボットの動き [#mb50a946]
缶←←ボール
↑
↑超音波センサ
ロボット
上図のように超音波センサを向ける。
ボールが通り過ぎた時 : 超音波センサはボールを感知→0.8秒停...
1. ボールが跳ね返ったとき
缶→→ボール
↑
↑超音波センサ
ロボット
超音波センサで再び感知→ロボット直進→ボールをつかむ→ライン...
2. ボールが左に流れてしまったとき
2.1 跳ね返ったボールを感知しない
ボール←←
缶
↑
↑超音波センサ
ロボット
超音波センサはボールを感知しない→ロボット左へ向かう
2.2 缶の左側に向かい、ボールを探索
ボール
↑↑↑↑↑↑ 缶
↑↑↑↑
↑↑ 矢印は、超音波センサが作用する範囲
ロボット
☆缶を感知しない程度に、ロボット(超音波センサ)を回転させる。
-ボールが見つかった場合
ボールの方を向き、直進。その後ボールをつかむ
-ボールが見つからなかった場合
ロボットを下図の向きに戻して、少し進んで、再び☆の動作
缶
↑
↑ 矢印の方へ向く
ロボット
*「ボールを缶に当てるロボット」のプログラム [#k14bab2c]
自分の書いたプログラムではないので、詳しく説明はできませ...
#define straight OnFwd(OUT_BC,30);
#define turn_left OnFwd(OUT_B,30);Off(OUT_C);
#define turn_right OnFwd(OUT_C,30);Off(OUT_B);
#define TURN_LEFT OnFwd(OUT_B,30);OnRev(OUT_C,30);
#define TURN_RIGHT OnFwd(OUT_C,30);OnRev(OUT_B,30);
#define THRESHOLD 35
#define get OnFwd(OUT_A,45);Wait(300);Off(OUT_A);Wait(10...
#define open OnRev(OUT_A,45);Wait(300);Off(OUT_A);Wait(1...
const float diameter=5.45;
const float track=10.35;
const float pi=3.1415;
void fwdDist(float d) { //dcm進む
long angle;
angle=d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,30,angle,0,true,true);
}
void turnAng(long ang) { //ang度右旋回
long angle;
angle=track/diameter*ang;
RotateMotorEx(OUT_BC,30,angle,100,true,true);
}
int searchDirection(long ang) { //最短距離の物体の方を向く
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=55;
angle=(track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,30,-100);
while(MotorTachoCount(OUT_B)<=angle){
if(SensorUS(S4)<d_min){
PlaySound(SOUND_CLICK);
d_min=SensorUS(S4);
tacho_min=MotorTachoCount(OUT_B);
}
}
OnFwdSyncEx(OUT_BC,30,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S4)<=...
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;
}
task main() {
SetSensorLight(S3);
SetSensorLowspeed(S4);
int catch=0;
straight;
Wait(500);
RotateMotor(OUT_A,30,-80);
while(true){
if(SensorUS(S4)>=12){ //超音波センサ値 >= 12 つまり、...
if(SENSOR_3<THRESHOLD-9){
TURN_LEFT;
}else if(SENSOR_3<THRESHOLD-5){
turn_left;
}else if(SENSOR_3<THRESHOLD+5){
straight;
}else if(SENSOR_3<THRESHOLD+9){
turn_right;
}else{
TURN_RIGHT;
}
}else{ //センサがボールを感知したとき
Off(OUT_BC);
get;
Wait(1000);
catch++;
if(catch==1){
RotateMotorEx(OUT_BC,40,-200,0,true,false);
RotateMotorEx(OUT_BC,40,200,-100,true,false);
Off(OUT_BC);
Wait(500);
RotateMotorEx(OUT_BC,40,700,0,true,false);
Off(OUT_BC);
Wait(500);
int d=searchDirection(130);
PlaySound(SOUND_UP);
if(d<=50){ //超音波センサが缶を感知した時した時
Off(OUT_BC); //↓ここからボールを放つ動作
Wait(500);
RotateMotorEx(OUT_BC,40,-300,0,true,false); //下がる
Wait(6000);
open; //アームを開く
RotateMotorEx(OUT_BC,90,340,0,true,false); ...
Off(OUT_BC); //急停止、このときボールを放つ
Wait(1000);
RotateMotorEx(OUT_BC,65,-730,0,true,true); /...
RotateMotorEx(OUT_BC,40,150,100,true,false);...
RotateMotor(OUT_A,60,50); //アームを閉じる
straight; //少しの直進
Wait(2500);
}
}
}
}
}
*「ボールを回収するロボット」のプログラム [#z16e8d55]
自分が担当したのは、主にこの部分です。
**マクロ [#m15c1f4b]
#define SPEED 50 //Distなどのモータのパワーに、幅広く使用
#define SPEED_SLOW 30 //関数hand,turnAngなどで使用
#define THRESHOLD 45 //閾値
#define STEP 1 //Waitの引数として使う
#define STOP 3 //「ボールがセンサの前を横切った」かどう...
**関数 [#i3681e58]
***hand...ボールをつかむ [#y627f394]
sub hand(int ang, int plmi) { //ボールをつかむ動作
/*plmi = 1 → アーム開
plmi = -1 → アーム閉 */
RotateMotor(OUT_A, SPEED_SLOW * plmi, ang);
}
***Dist...指定した距離進む [#gc626d6c]
const float diameter=5.45; //タイヤの直径(cm)
const float pi=3.1415; //円周率
const float track = 10.35; //タイヤのトレッド幅
sub Dist(float d, int fwd_or_rev){ //dcm前転(後転)
/*fwd_or_rev = 1 → 前転
fwd_or_rev = -1 → 後転 */
long angle;
angle = d / (diameter * pi) * 360.0;
RotateMotorEx(OUT_BC, SPEED * fwd_or_rev, angle, 0, tru...
Wait(100);
}
***turnAng...指定した角度旋回する [#l31c83ee]
sub turnAng(int ang, int R_or_L) //ang度旋回
{
/* R_or_L = 1 → 時計回り
R_or_L = -1 → 半時計回り */
long angle;
angle = track / diameter * ang;
RotateMotorEx(OUT_BC, SPEED_SLOW , angle, 100* R_or_L, ...
Wait(100);
}
***searchDirection...最短距離の方を向く [#wd8a8862]
int searchDirection(int ang) //ボールを探す関数
{
SetSensorLowspeed(S1);
long angle, tacho_min = 0, tacho_corr;
int d_min;
d_min= 300; //仮の最小値
angle = (track / diameter) * ang;
turnAng(10, 1); //10度右旋回(缶の誤認を防ぐため、10度...
ResetTachoCount(OUT_B);
OnFwdSync(OUT_BC, SPEED, -100);
while(MotorTachoCount(OUT_B) <= angle) { //angleだけ左...
if (SensorUS(S1) < d_min) {
d_min = SensorUS(S1);
tacho_min = MotorTachoCount(OUT_B);
}
}
if (d_min < 30) { //「超音波センサの示した最小値」が30...
OnFwdSyncEx(OUT_BC,SPEED, 100, RESET_NONE); //最小値...
until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(...
}
else { //30以上のとき
turnAng(ang - 10, 1); //ang-10だけ右旋回(関数開始前と...
}
Off(OUT_BC); Wait(500); //0.5秒間停止
return d_min; //「超音波センサの示した最小値」を返す
}
***ライントレースする [#l58de8e2]
関数「trace」で使う関数
sub turn_left(int speedA, int speedB) { //左に曲がる
OnFwd(OUT_B,speedA); //引数speedAのパワーで、モータ...
OnRev(OUT_C,speedB); //引数speedBのパワーで、モータ...
}
sub turn_right(int speedA, int speedB) { //右に曲がる
OnFwd(OUT_C,speedB); //引数speedBのパワーで、モータ...
OnRev(OUT_B,speedA); //引数speedAのパワーで、モータ...
}
sub go_forward(int speed3) { //まっすぐ進む
OnFwd(OUT_BC,speed3); //speed3のパワーで、モータBCを...
}
関数trace...「time秒以内、かつ、TachoCount < target」の間...
1.交差点かどうかを見極める際に、タイマーではなくタイヤの...
例えば、左旋回した時(光センサ値 < THRESHOLD - 15)のタイヤ...
2.「ライントレースを終了する時間」を設定できる
ライントレースを続ける時間を、引数timeで与えられる。1の方...
sub trace(int time, long target, int speed3) {
SetSensorLight(S2);
int speed1 = 25; //モータのパワー用
int speed2 = 20; //モータのパワー用(speed1より少し...
long tacho = 0; //「その場で左旋回する」動作をし...
long t; //タイマーを使うときに使用
t = CurrentTick(); //ここで現在時刻をtに代入(CurrentTi...
while (CurrentTick() - t < time && tacho < target) {
if (SENSOR_2 < THRESHOLD -15){ //光センサの数...
turn_left(speed1, speed1); //左右のモータをspeed1...
} else if (SENSOR_2 < THRESHOLD -7) { //光センサの数...
tacho = 0; //tacho(turn_left1し続けた時の、タイヤ...
turn_left(speed1, speed2); //右モータをspeed1、左...
} else if (SENSOR_2 < THRESHOLD +7){ //光センサの数値...
tacho = 0; //tachoを0にする
go_forward(speed3); //左右のモータをspeed3にして...
} else if (SENSOR_2 < THRESHOLD +15){ //光センサの数...
tacho = 0; /*tachoを0にする*/
turn_right(speed1, speed2); /*右モータをspeed2、左...
} else { /*光センサの数値が、THRESHOLD+15より高い...
tacho = 0; /*tachoを0にする*/
turn_right(speed1, speed1); /*左右のモータをspeed1...
}
Wait(STEP); //STEP(0.001)秒だけ動く
if (SENSOR_2 >= THRESHOLD-15) { /*光センサの数値が、...
ResetTachoCount(OUT_B); /*モータの回転数をリセット...
}
tacho += MotorTachoCount(OUT_B); /*tachoに、モータの...
}
Off(OUT_BC); Wait(STEP);
PlaySound(SOUND_CLICK);
}
**メイン関数 [#z16c056e]
***実際の動作 [#ac6032c0]
プログラムの動きを画像で示した。(/*(1)*/の画像は、/*(1)*/...
/*--------(1)----------*/
#ref(2015b/Member/rmsun/Mission3/ロボコン1.gif,480*332)
/*--------(2)---------*/
#ref(2015b/Member/rmsun/Mission3/2015b-missiona.jpg,480*3...
/*--------(3)---------*/
#ref(2015b/Member/rmsun/Mission3/ロボコン3.gif,480*332)
/*--------(4)----------*/(←この動作は、改善の余地あり)
#ref(2015b/Member/rmsun/Mission3/ロボコン4.gif,480*332)
***mainのプログラム [#y460ebf3]
task main() {
/*-------------(1)--------------*/
long t;
Wait(30000); //30秒待機
Dist(48, 1); //48cm前進
turnAng(91, -1); //90度左旋回
SetSensorLowspeed(S1); //超音波センサ起動
int ball = 0; //変数「ball」...超音波センサの値 < 30 ...
while (1) { //ボールが通過する様子を調べる
if (SensorUS(S1) < 30) { /*センサ値 < 30(ボールがセン...
ball++;
}
if (ball == STOP) { //通過した回数がSTOP(=3)になったら
break; //ループを抜け出す
}
Wait(STEP); //STEP(0.001)秒間
}
Wait(800); //0.8秒間待機
PlaySound(SOUND_CLICK); //次の動作に入ることを知らせる
t = CurrentTick();
while (CurrentTick() - t < 2000) { //2秒の間繰り返す
if (SensorUS(S1) < 30) { //センサ値 < 30のとき
ball++; //ball + 1 ..つまりball = STOP + 1
break; //ループを抜け出す
}
Wait(STEP);
}
/*------------(2)----------------*/
if (ball == STOP + 1) { //超音波センサ < 30を示すまで...
OnFwd(OUT_BC, SPEED_SLOW); //SPEED_SLOW(=30)のパワー...
while (ball < STOP + 2) {
if (SensorUS(S1) < 30) {
ball++;
}
}
Off(OUT_BC); //1秒停止
Wait(1);
Dist(35, 1); //35cm前進
hand(70, -1); //ボールをつかむ
/*-----(黄線)-----*/
turnAng(30, -1); //30度右旋回(ライントレースの黒線に...
Dist(8, 1); //8cm前進(ライントレースの黒線に入るため)
/*-----(緑線)-----*/
trace(20000, 15, 40); /*「20秒以内 && TachoCount < 15...
/*-----(水色線)---*/
trace(7000, 1000, 50); /*「7秒以内 && TachoCount < 10...
/*------(青線)----*/
trace(15000, 10, 40); /*「15秒以内 && TachoCount < 10...
/*------(紫線)----*/
turnAng(75, 1); //75度右旋回
Dist(15,1); //15cm前進
/*------(赤線)-----*/
turnAng(20, 1); //20度右旋回
SetSensorLight(S2); //光センサー起動
OnFwd(OUT_BC, SPEED); //黒線までSPEED(=50)のパワーで...
while (SENSOR_2 > THRESHOLD - 15) {}
Off(OUT_BC); //停止
hand(70, 1); //ボールを離す
}
/*-------------(3)------------*/
else {
turnAng(90, -1); //90度左旋回
Dist(40, 1); //40cm進む
turnAng(90, 1); //90度右旋回
Off(OUT_BC); Wait(STEP); //0.001秒間停止
while (1) {
int d = searchDirection(60); //60度の範囲でボールを...
if (d < 30) { /*searchDirection(60)の返り値 < 30のと...
Dist(20, 1); //20cm進む
hand(70, -1); //ボールをつかむ
break; //ループから抜け出す
}
else {
OnFwd(OUT_BC, SPEED); //0.5秒進む
Wait(500);
searchDirection(0); /*searchDirectionを2連続で使う...
}
}
/*------------(4)---------------*/
trace(10000, 10000, 45); //どこかのラインにのる,ロボッ...
Dist(40, 1); //40cm進む、
SetSensorLight(S2); //光センサー起動
OnFwd(OUT_BC, SPEED); //SPEED(=50)のパワーで、黒線に到...
while (SENSOR_2 > THRESHOLD - 15) {}
trace(3000, 10000, 40); //「3秒以内 && TachoCount < 10...
hand(70, 1); //ボールを離す
}
}
*コメント[#od03dfdc]
**工夫点 [#oe6a4c64]
-ボールが缶に当たった後の「ボールが跳ね返った場合」と「ボ...
-「ボールがあったらボールの方向を向き、ボールがなかったら...
**反省点 [#b3dc3c0c]
-/*(4)*/の部分が不安定。ボールの位置によって、ゴールに到...
-超音波センサに頼りすぎた。超音波センサが予期せぬ動作をし...
-缶より手前(ロボット側)にボールが飛んできたとき、缶に当た...
-「二個目のボールを探すためのプログラム」が書けなかった(...
-投げる側のロボットを超音波センサが誤認するときがあった。
-投げる側のロボットと衝突するときがあった。
**感想 [#q20ee53a]
-ハードウェアの性能によって、ロボットのできることが大きく...
-超音波センサがうまく機能しないとき、ロボットの誤作動を防...
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
