- 追加された行はこの色です。
- 削除された行はこの色です。
目次
#contents
*機構 [#b134b320]
*課題 [#i4657ad2]
*プログラム [#f0357afe]
**基本動作の定義付け [#s2d15de5]
#define set_power_L SetPower(OUT_A,1);SetPower(OUT_C,1); // 速度を遅く設定する
#define THRESHOLD 45 // しきい値の設定
#define go_forward OnFwd(OUT_AC); // 前進
#define turn_right0 OnFwd(OUT_A);OnRev(OUT_C); // 右旋回
#define turn_right1 OnFwd(OUT_A);Off(OUT_C); // 右回転
#define turn_left1 Off(OUT_A);OnFwd(OUT_C); // 左回転
#define turn_left0 OnRev(OUT_A);OnFwd(OUT_C); // 左旋回
#define STEP 1 // 動作を一回行う
#define U_turn OnFwd(OUT_A);OnRev(OUT_C);Wait(180);Off(OUT_AC); // Uターンをする
**コップを持ち上げるプログラムの定義付け [#m35472aa]
#define cup_up OnFwd(OUT_B);Wait(130);Off(OUT_B); // コップを持ち上げる
#define cup_down OnRev(OUT_B);Wait(130);Off(OUT_B); // コップを下す
**変数 [#j93408b5]
int number=0; // グローバル変数。コップの色に応じて1〜3の値をとり、点Aからのパターン分岐を決める
**サブルーチン [#d9539c94]
***ライントレース [#nf4e7543]
/*論理積を使ったライントレースのサブルーチン*/
sub line(){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転をする
}else{go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
***紙コップを掴んだ後の動作 [#o7288d92]
/*紙コップを掴んだ後に「点A」に辿り着いてから行う操作のサブルーチン*/
sub carry_cup(){
if(number==1){
while((SENSOR_2<47)&&(SENSOR_2>35)){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値、右回転する
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値を十た時、右回転する
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
go_forward;
Wait(10); // 紙コップに完全に接触するまで前進。そこからの位置調整も要調査
Wait(10); // 紙コップに完全に接触するまで前進
Off(OUT_AC);
cup_down;
U_turn;
while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップBかCを認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値を十た時、右回転する
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値がそれ以外の時は前進
}
}
}else if(number==2){
while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){ // 交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
turn_right0;
Wait(75); // コップ2の逆方向を向かせる
go_forward;
Wait(10);
U_turn; // 交差点でそのまま左折するとコップ2を押してしまう為
while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップ2を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward;
Wait(STEP);
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
go_forward;
Wait(20); // 紙コップに完全に接触するまで前進。そこからの位置調整も要調査
Wait(20); // 紙コップに完全に接触するまで前進
Off(OUT_AC);
cup_down;
OnRev(OUT_AC);
Wait(130);
while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる
while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){ // 交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
turn_left0;
Wait(75); // 軸にそって紙コップBかCに向かせる
while((SENSOR_2>35)&&(SENSOR_2<47)){ // 紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
}else{
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
go_forward;
Wait(10);
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる
go_forward;
}
turn_right0;
Wait(75);
while((SENSOR_2>35)&&(SENSOR_2<47)){ // 紙コップを認識するまで
go_forward;
}
go_forward;
Wait(25);
Off(OUT_AC);
cup_down;
OnRev(OUT_AC);
Wait(110);
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる
OnRev(OUT_AC);
}
turn_right0;
Wait(75);
}
while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
}
}
**開始直後の紙コップAを掴み振り向くまでの動き [#l38033cd]
void start_dash(){
go_forward;
Wait(120); // スタート地点からでる為の動き
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){
go_forward; // 交差点を認識するまで前進
}
turn_right0;
Wait(75);
while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップを認識するまで
while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
go_forward;
Wait(100); // コップに接触するまで
Wait(100); // コップに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){ // 白
number=1;
}else if((SENSOR_2<47)&&(SENSOR_2>37)){
number=2;
}else{
number=3;
} // センサーの値に応じてnumberに値を入れる
} // センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
}
**全体としてのプログラム [#vbcd7354]
task main(){
SetSensor(SENSOR_1,SENSOR_LIGHT);
SetSensor(SENSOR_2,SENSOR_LIGHT);
SetSensor(SENSOR_3,SENSOR_LIGHT);
set_power_L;
int cross=0; // 交差点通過回数のカウンタ
start_dash(); // コップAを掴む
carry_cup();
go_forward;
Wait(50); // コップBに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){
number=1;
}else if((SENSOR_2>35)&&(SENSOR_2<47)){
number=2;
}else{
number=3;
} // センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
ClearTimer(0); // タイマー設置
while(Timer(0)<40){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
carry_cup();
go_forward;
Wait(50); // コップCに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){
number=1;
}else if((SENSOR_2>35)&&(SENSOR_2<47)){
number=2;
}else{
number=3;
} // センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
ClearTimer(0);
while(Timer(0)<20){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転する
}else{
go_forward;
Wait(STEP);
Wait(STEP); // センサーの値が条件を満たさない時は前進
}
}
carry_cup();
Off(OUT_AC);
}
*サブ機のプログラム [#o70cafd7]
task main()
{
OnFwd(OUT_AC);
Wait(540);
OnFwd(OUT_C); OnRev(OUT_A); Wait(100); // 左旋回
OnFwd(OUT_AC);
Wait(150); // 紙コップをライン上に置く
OnRev(OUT_AC);
Wait(100); // 回転した時にアームが紙コップに当たらないようにするためバックする
OnFwd(OUT_C); OnRev(OUT_A); Wait(160); // 左旋回
OnFwd(OUT_AC);
Wait(155);
OnFwd(OUT_A); OnRev(OUT_C); Wait(150); // 右旋回
OnFwd(OUT_AC);
Wait(350);
OnFwd(OUT_A); OnRev(OUT_C); Wait(270); // 紙コップを持ちながら一回転
OnFwd(OUT_AC); // 紙コップをライン上に置く
Wait(90);
OnRev(OUT_AC);
Wait(100);
Off(OUT_AC); // もう一つの機体の邪魔にならないようにラインから離れる
}
*感想・反省 [#z2b04749]