[[2012a/MemberOnly/進行状況A]] [[ロボティクス入門ゼミ]] #contents *プログラム [#vde8e72f] /* 2012/7/27 ITY 借用 Haxa */ /* 2012a_robo robokon12 */ /*------------------------------------------------------------------- 別名定義 -------------------------------------------------------------------*/ #define PWM 80 // PWM(力の大きさ) #define PWM_D 15 // PWM(力の大きさ) #define S_BR 40 // 黒線のしきい値 #define S_WH 60 // 白線のしきい値 #define DIF 6 // 差 #define ON 1 // オン #define OFF 0 // オフ #define EN_D 40 // 円の直径 #define TY_B 150 // タイヤベース #define TY_D 55 // タイヤの直径 #define PAI 3.14 // 円周率3.14 #define FF 1 // 前進 #define FR 2 // 右旋回 #define RF 3 // 左旋回 #define RR 4 // 後進 #define BB 5 // ブレーキ /*------------------------------------------------------------------- 関数宣言 -------------------------------------------------------------------*/ void LCD_D(); // LCDの表示 void TRACE(); // ライントレース void START(); // スタート void mt_ct(int mt_ct,int ang_d); /*------------------------------------------------------------------- グローバル変数 -------------------------------------------------------------------*/ int s_lowse1,s_lowse2,s_light1,s_light2; // 変数宣言 /*------------------------------------------------------------------- メインルーチン -------------------------------------------------------------------*/ task main() { SetSensorLight(IN_4); // 光センサの宣言 SetSensorLight(IN_3); // 光センサの宣言 SetSensorTouch(IN_2); // タッチセンサの宣言 SetSensorLowspeed(IN_1); // 超音波センサ int speed,diff,nairin,in; // 変数宣言 float p; // フロートで宣言 int temp; // 一時退避 int cnt=0,cnt_t0=0,cnt_t1=0,mode=0; // 変数の初期化 int temp0=50,temp1=50,cnt_t=0; // 変数の初期化 int time=15,pw=80; // 時間15ms_PMW80% temp = SensorUS(IN_1); // 初期の値を取得 while(true){ // 繰り返す s_lowse1 = SensorUS(IN_1); // 変数の代入 s_lowse2 = Sensor(IN_2); // 変数の代入 s_light1 = Sensor(IN_3); // 変数の代入 s_light2 = Sensor(IN_4); // 変数の代入 SendRemoteNumber(MAILBOX2,true,2); // 2を送る(掴む動作の開始) while(true){ ReceiveRemoteNumber(MAILBOX3,true,in);//メッセージを受けとる if(in==3){ // 3が来るまでまで無限ループ break; // 抜ける } SendRemoteNumber(MAILBOX2,true,1); // 1を送る(掴む動作の停止) } // START(); // 開始 mt_ct(FF,100); // 前進 mt_ct(FF,200); // 前進 mt_ct(RF,93); // 90度左旋回 mt_ct(FF,640); // 前進 p = 1; // pに1を代入 while(SensorUS(IN_1) > 8){ // 超音波センサの値が8以上の時 mt_ct(RF,(10*p+6)); // 左旋回 if((temp+3) > SensorUS(IN_1)){ // センサが初期の値より小さい時 p = p * 0.6; // 回転角を小さくする RotateMotor(OUT_AC,pw,60);// 前進 mt_ct(FR,(15*p)); // 右旋回 } else if(p < 1){ // pが1より小さい時(反応があった時) mt_ct(FR,15); // 右旋回 p = 1; // pを初期化 } } OnFwdSync(OUT_AC,80,0); // 前進する while((Sensor(IN_2) == OFF)){}; // タッチセンサに反応があるまで前進 Off(OUT_AC); // モータの停止 SendRemoteNumber(MAILBOX2,true,2); // 2を送る(缶を降ろす動作) Wait(6000); // 6秒間待つ OnRevSync(OUT_AC,100,0); // 1秒間後退する Wait(1000); mt_ct(RF,200); // 90度左旋回 mt_ct(FF,600); // 前進 p = 1; // pの初期化 while(SensorUS(IN_1) > 6){ // 超音波センサの値が6以上の時 mt_ct(RF,10); // 左旋回 if((temp+5) > SensorUS(IN_1)){ // センサが初期の値より小さい時 RotateMotor(OUT_AC,pw,200);// 前進 mt_ct(FR,(10*p*10)/10); // 右旋回 p = p * 0.6; // 回転角を小さくする } else p = 1; // pの 初期化 } OnFwdSync(OUT_AC,80,0); // 前進する while((Sensor(IN_2) == OFF)){}; // タッチセンサに反応があるまで前進 Off(OUT_AC); // 停止 while(true){}; // 停止 // ResetScreen(); // LCDの初期化 // LCD_D(); // LCDの表示 // TRACE(); // トレースの開始 } } /*------------------------------------------------------------------- サブルーチン -------------------------------------------------------------------*/ /* 進む距離回転する角度計算用関数 */ void mt_ct(int mt_ct,int ang_d) { int r,d,g,mt; int iNum1=0,iNum2=0; r = TY_D * PAI; //タイヤの円周 // 進む距離計算 if(mt_ct == FR || mt_ct == RF){ d = TY_B * PAI * ang_d / 360; } else{ d = ang_d; } mt = 360 * d / r; //進む距離からパルス数計算 switch(mt_ct){ case FF: //前進 OnFwdSyncEx(OUT_AC,PWM,0,RESET_ALL); break; case FR: //右旋回 OnFwdSyncEx(OUT_AC,PWM,100,RESET_ALL); break; case RF: //左旋回 OnFwdSyncEx(OUT_AC,PWM,-100,RESET_ALL); break; case RR: //後進 OnRevSyncEx(OUT_AC,PWM,0,RESET_ALL); break; case BB: //ブレーキ Off(OUT_AC); break; default: Off(OUT_AC); break; } while(abs(iNum1) < mt || abs(iNum2) < mt){//左右のパルス数が目標値になるまで待つ iNum1 = MotorTachoCount(OUT_A); iNum2 = MotorTachoCount(OUT_C); } Off(OUT_AC); //両輪停止 Wait(500); //500ms待つ } /*------------------------------------------------------------------- 進む距離回転する角度計算用関数 はHexaさんより借用 -------------------------------------------------------------------*/ void LCD_D(){ // LCDの表示 while(true){ ResetScreen(); NumOut(10,48,SensorUS(IN_1),0); // 超音波センサの値を表示 NumOut(10,40,SensorUS(IN_2),0); // 超音波センサの値を表示 NumOut(10,32,Sensor(IN_3),0); // 光センサの値を表示 NumOut(10,24,Sensor(IN_4),0); // 光センサの値を表示 Wait(100); } } void START(){ // 後退、前進、回転を繰り返す int i; // 変数宣言 for(i=0;i<3;i++){ // 3回繰り返す OnRev(OUT_AC,PWM); // 後退する Wait(100); // 0.1秒待つ while((s_light1 > S_WH) && (s_light2 > S_WH)){ OnFwd(OUT_AC,PWM); // 前進 } if(s_light1 < S_BR){ Off(OUT_C); // モータCの停止 while(s_light2 < S_BR)OnFwd(OUT_A,PWM); } else if(s_light2 < S_BR){ Off(OUT_A); // モータAの停止 while(s_light1 < S_BR)OnFwd(OUT_C,PWM); } Wait(100); // 1秒間待つ } } void TRACE(){ // 黒線のトレース int nairin,mode; // 変数宣言 mode=0; // 変数の初期化 // ResetTachoCount(OUT_AB); // タコメータのカウント nairin = ((PWM*(EN_D-TY_B)*10)/(EN_D+TY_B)/10); // 内輪差の計算 if((s_light1 > S_WH) && (s_light2 > S_WH)){ OnFwd(OUT_A,nairin); // 前進 OnFwd(OUT_C,PWM); // 前進 } else { if(s_light1 < S_BR){ OnFwd(OUT_A,nairin*0.8);// 前進 OnFwd(OUT_C,PWM); // 前進 if(mode/2 == 1){ mode++; // モードの増加 } } else if(s_light2 < S_BR){ OnFwd(OUT_A,PWM); // 前進 OnFwd(OUT_C,nairin*0.8);// 前進 if(mode/2 == 0){ mode++; } } } }