- 追加された行はこの色です。
- 削除された行はこの色です。
[[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++;
}
}
}
}