![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
目次
2019b/Mission2を参照。
２人目のコースを走らせた。
タイムを意識してロボットを製作した。
具体的には、ロボットの重心を下げることで、スピードを上げても安定して動くようなロボットに仕上げた。
スピードを出しても安定して動くように重心をなるべく低くしようとした結果、アーム用のモーターが後ろ向きについてしまった。
この結果、ボールをつかむときには、Uターンをする必要があるので、そのときの誤差を埋めるために、アームは、コース走行中にボールに当たらないギリギリまで大きくした。
今回は、ボールまでの距離と角度を知るために超音波センサーを使用した。ボールの一番太いところと高さが同じくらいになるように取り付けた。いろいろ試した結果、距離を正しく出すにはこの位置が一番うまくいったのでこの位置にした。
光センサーはなるべく前方に出して、いち早くコースの状況を判別できるようにした。しかし、超音波センサーより前には取り付けできないので、この位置に付けることにした。
#define Speed 80
#define go(p) OnFwdSync(OUT_AC, p, 0) //直線
#define b 35 //黒
#define bw 40 //黒灰
#define wb 43 //白灰
#define w 48 //白
#define up OnFwd(OUT_B, -30);Wait(1000) //ボールキャッチャー上 げる
#define down OnFwd(OUT_B, 30);Wait(1000) //ボールキャッチャー 下げる
#define L 1 //左
#define R 0 //右
//Aが左、Cが右
long t_x,t0;
const float cyokkei = 5.45; //タイヤ直径
const float haba = 11.05; //タイヤ間の距離
const float pi = 3.14; //π
const float P = 40;
const float I = 40;
const float D = 90;
void follow_line(long tmin, long tmax, int LorR) //ライントレ ース
{
SetSensorLight(S3);
t_x = CurrentTick();
t0 = CurrentTick(); //この関数が動き始めた時間
float S_l;
int M_l; //左側トレース １、右側トレース０
int M_r;
if (LorR == 1){
M_l = 0x00; //OUT_A
M_r = 0x02; //OUT_C
}else if(LorR == 0){
M_l = 0x02;
M_r = 0x00;
}
while(CurrentTick()-t0 < tmin){ // long tmin: スタートから の時間がtmin以下では、交差点を無視してライントレースする
S_l = SENSOR_3;
if(S_l < b){ //黒
OnFwd(M_r,Speed);
Off(M_l);
if(CurrentTick()-t_x > 400){
OnFwd(M_l,-Speed);
}
}else{
if(S_l > w){ //白
OnFwd(M_l,Speed);
Off(M_r);
if(CurrentTick()-t_x > 400){
OnFwd(M_r,-Speed);
}
}else if(S_l > wb){ //白灰
OnFwd(M_l,Speed);
OnFwd(M_r,Speed*((1-((S_l-wb)/(w-wb)))));
t_x = CurrentTick();
}else if(S_l < bw){ //黒灰
OnFwd(M_r,Speed);
OnFwd(M_l,Speed*((1-((S_l-b)/(bw-b)))));
t_x = CurrentTick();
}else{ //灰
go(Speed);
t_x = CurrentTick();
}
}
}
t_x = CurrentTick(); // long tmax: スタートからの時間が tmax経てば、交差点に達しなくても停止
while(CurrentTick()-t_x < 230 && CurrentTick()-t0 < tmax) { //交差点で止まる
S_l = SENSOR_3;
if(S_l < b){ //黒
OnFwd(M_r,Speed);
Off(M_l);
}else{
t_x = CurrentTick();
if(S_l > w){ //白
OnFwd(M_l,Speed);
Off(M_r);
}else if(S_l > wb){ //白灰
OnFwd(M_l,Speed);
OnFwd(M_r,Speed*((1-((S_l-wb)/(w-wb)))));
}else if(S_l < bw){ //黒灰
OnFwd(M_r,Speed);
OnFwd(M_l,Speed*((1-((S_l-b)/(bw-b)))));
}else{ //灰
go(Speed);
}
}
}
Off(OUT_AC); //交差点で止まる
}
void go_cm(float d) //直進（距離指定(cm)）
{
long angle;
angle = d/(cyokkei*pi)*360.0;
RotateMotorExPID(OUT_AC,Speed,angle,0,true,true,P,I,D);
}
void turn_ang(long ang) //時計回り旋回
{
long angle;
angle = (haba/cyokkei)*ang;
RotateMotorExPID(OUT_AC,Speed,angle,100*sign(ang),true, true,P,I,D);
}
float search_D(long ang) //最小距離を返す（探す対象の角度）
{
SetSensorLowspeed(S2);
long angle,min_ang;
float min_d;
min_d = 100000;
angle = (haba/cyokkei)*ang;
turn_ang(-ang/2);
ResetTachoCount(OUT_AC);
int kakudo;
float S_us;
kakudo = 0;
Wait(800);
while(kakudo <= ang){
turn_ang(ang/15);
Wait(900);
S_us = SensorUS(S2); //距離を計測
if (S_us < min_d){
min_d = S_us;
min_ang = MotorTachoCount(OUT_C);
}
kakudo = kakudo + ang/15;
}
Wait(1000);
turn_ang(-(MotorTachoCount(OUT_C)-min_ang -69)); //補正値 62
Off(OUT_AC);
Wait(100);
return min_d;
}
void fetch_ball(long tmax) //ボールを掴む
{
t0 = CurrentTick(); //この関数が動き始めた時間
while (tmax > CurrentTick()-t0){ // long tmax: スタートか らの時間がtmax経てば、ボールをつかめなても戻ってくる
float d = search_D(90);
while (d > 30){ //距離を吟味する
d = search_D(90); //ボールに近づく
}
go_cm(d-20);
Wait(700);
OnFwd(OUT_B,-10);
Wait(1000);
turn_ang(180+10);
OnFwd(OUT_B,10);
Wait(1000);
Off(OUT_B); //ボールをつかむ
break;
}
}
//メイン -------------------------------------------------------------
task main()
{
const int wait_t = 1000;
follow_line(2500,1000,L); //Jをスタート。交差点Hで止まる。
Wait(wait_t);
fetch_ball(60000); //ボールをキャッチ。
follow_line(0,1000,R); //交差点Hで止まる。
Wait(wait_t);
follow_line(0,1000,R); //交差点Gで止まる。
Wait(wait_t);
go_cm(2.5); //交差点Gを直進。
follow_line(1000,2000,R); //E,Fを通過し、交差点Gで止まる。
Wait(wait_t);
go_cm(2.5); //交差点Gを直進。
follow_line(3000,1000,L); //ゴール
}
今回は、ライントレースを行う関数を制作した。 なるべく速くコースを走らせたかったので、なるべくモーターの逆回転（進行方向と逆向き）を抑えるプログラムにしたかった。そこで、逆回転を行うプログラムを交差点を判断しない方に組み込み、逆回転を行わないプログラムを交差点を判断する方にした。さらに、スピードアップのために比例制御のようなものも導入した。
また、線の左右どちらをトレースするかを関数の引数で切り替えられるようにした。
このような関数になった。
follow_line(交差点を無視する時間, プログラムを止める時間, 線の左右どちらをトレースしたいか(L or R))
プログラム
#define b 35 //黒 #define bw 40 //黒灰 #define wb 43 //白灰 #define w 48 //白
#define L 1 //左
#define R 0 //右
void follow_line(long tmin, long tmax, int LorR) //ライントレ ース
{
SetSensorLight(S3);
t_x = CurrentTick();
t0 = CurrentTick(); //この関数が動き始めた時間
float S_l;
int M_l; //左側トレース １、右側トレース０
int M_r;
if (LorR == 1){
M_l = 0x00; //OUT_A
M_r = 0x02; //OUT_C
}else if(LorR == 0){
M_l = 0x02;
M_r = 0x00;
}
while(CurrentTick()-t0 < tmin){ // long tmin: スタートから の時間がtmin以下では、交差点を無視してライントレースする
S_l = SENSOR_3;
if(S_l < b){ //黒
OnFwd(M_r,Speed);
Off(M_l);
if(CurrentTick()-t_x > 400){
OnFwd(M_l,-Speed);
}
}else{
if(S_l > w){ //白
OnFwd(M_l,Speed);
Off(M_r);
if(CurrentTick()-t_x > 400){
OnFwd(M_r,-Speed);
}
}else if(S_l > wb){ //白灰
OnFwd(M_l,Speed);
OnFwd(M_r,Speed*((1-((S_l-wb)/(w-wb)))));
t_x = CurrentTick();
}else if(S_l < bw){ //黒灰
OnFwd(M_r,Speed);
OnFwd(M_l,Speed*((1-((S_l-b)/(bw-b)))));
t_x = CurrentTick();
}else{ //灰
go(Speed);
t_x = CurrentTick();
}
}
}
t_x = CurrentTick(); // long tmax: スタートからの時間が tmax経てば、交差点に達しなくても停止
while(CurrentTick()-t_x < 230 && CurrentTick()-t0 < tmax) { //交差点で止まる
S_l = SENSOR_3;
if(S_l < b){ //黒
OnFwd(M_r,Speed);
Off(M_l);
}else{
t_x = CurrentTick();
if(S_l > w){ //白
OnFwd(M_l,Speed);
Off(M_r);
}else if(S_l > wb){ //白灰
OnFwd(M_l,Speed);
OnFwd(M_r,Speed*((1-((S_l-wb)/(w-wb)))));
}else if(S_l < bw){ //黒灰
OnFwd(M_r,Speed);
OnFwd(M_l,Speed*((1-((S_l-b)/(bw-b)))));
}else{ //灰
go(Speed);
}
}
}
Off(OUT_AC); //交差点で止まる
}
まず、ボールがどこにあるかを探すプログラムを作った。
授業中に配られたプリントに従うとうまくいかなかったので、書き直した。具体的には、距離の計測中にセンサーが動かないようなプログラムに変更した。
const float cyokkei = 5.45; //タイヤ直径
const float haba = 11.05; //タイヤ間の距離
const float pi = 3.14; //π
void turn_ang(long ang) //時計回り旋回
{
long angle;
angle = (haba/cyokkei)*ang;
RotateMotorExPID(OUT_AC,Speed,angle,100*sign(ang),true, true,P,I,D);
}
float search_D(long ang) //最小距離を返す（探す対象の角度）
{
SetSensorLowspeed(S2);
long angle,min_ang;
float min_d;
min_d = 100000;
angle = (haba/cyokkei)*ang;
turn_ang(-ang/2);
ResetTachoCount(OUT_AC);
int kakudo;
float S_us;
kakudo = 0;
Wait(800);
while(kakudo <= ang){
turn_ang(ang/15);
Wait(900);
S_us = SensorUS(S2);
if (S_us < min_d){
min_d = S_us;
min_ang = MotorTachoCount(OUT_C);
}
kakudo = kakudo + ang/15;
}
Wait(1000);
turn_ang(-(MotorTachoCount(OUT_C)-min_ang -69)); //補正値 62
Off(OUT_AC);
Wait(100);
return min_d;
}
上のプログラムでボールの位置がわかったので、手前まで移動する。しかし、超音波センサーはかなり気まぐれなので、とんでもない値を返してくることがある。そこで、明らかにおかしい値が返ってきたら、再度計測し直すプログラムを組み込んだ。
このロボットはアームが後ろにあるので、180度回転させる。アームの上げ下げはマクロを併用した。
void go_cm(float d) //直進（距離指定(cm)）
{
long angle;
angle = d/(cyokkei*pi)*360.0;
RotateMotorExPID(OUT_AC,Speed,angle,0,true,true,P,I,D);
}
void fetch_ball(long tmax) //ボールを掴む
{
t0 = CurrentTick(); //この関数が動き始めた時間
while (tmax > CurrentTick()-t0){ // long tmax: スタートか らの時間がtmax経てば、ボールをつかめなても戻ってくる
float d = search_D(90);
while (d > 30){ //距離を吟味する
d = search_D(90); //ボールに近づく
}
go_cm(d-20);
Wait(700);
OnFwd(OUT_B,-10);
Wait(1000);
turn_ang(180+10);
OnFwd(OUT_B,10);
Wait(1000);
Off(OUT_B); //ボールをつかむ
break;
}
}
左右どちらをトレースするかを簡単に指定できるので、メインプログラムは簡単に書けた。
task main()
{
const int wait_t = 1000; //交差点での待ち時間
follow_line(2500,1000,L); //Jをスタート。交差点Hで止まる。
Wait(wait_t);
fetch_ball(60000); //ボールをキャッチ。
follow_line(0,1000,R); //交差点Hで止まる。
Wait(wait_t);
follow_line(0,1000,R); //交差点Gで止まる。
Wait(wait_t);
go_cm(2.5); //交差点Gを直進。
follow_line(1000,2000,R); //E,Fを通過し、交差点Gで止まる。
Wait(wait_t);
go_cm(2.5); //交差点Gを直進。
follow_line(3000,1000,L); //ゴール
}
失敗した。
ボールをうまくつかめなかった。
授業前はうまくできていたので悔しい。
今回は、スピードを速くすることに重点を置いていた。実際、他のチームよりもライントレースは速かったと思う。しかし、ボールを探す時にはスピードを遅くしておけばよかった。スピードを上げた事による回転角の誤差は補正値を追加することで解決できると思っていたが、電池の残量で補正値がどんどん変化していって、根本的な解決にはなっていなかった。今回は、電池との戦いだったと思う。
2_robot_all.jpg
2_hand1.jpg
2_hand.jpg
2_sensor.jpg
