- 追加された行はこの色です。
- 削除された行はこの色です。
目次
#contents
*[[課題の説明>2014a/Mission1]] [#ofbcff9d]
*メンバー [#tbb423e0]
-bgpat: 主にプログラム担当 スタート→ゴール
-只の某: 主にロボット作成担当 ゴール→スタート
-bgpat: スタート→ゴール
-只の某: ゴール→スタート
*コースの作成 [#b0a98824]
&ref(2014a/Member/bgpat/Mission1/course.png,400x300);
このような画像ファイルを作成して何枚かのA4用紙に分けて印刷し、台紙に貼り付けました。
薄い部分は黒いペンで塗りつぶしました。
*ロボット本体 [#i40141cf]
**ドライブベース [#i6612d7b]
**アーム [#i7d72672]
*プログラム [#xe5408e4]
**定数の定義 [#rc89c212]
コースやロボットによって値は異なります。
/* トレースのスピード */
#define SPEED_HIGH 40
#define SPEED_MEDIUM 30
#define SPEED_LOW 25
#define SPEED_ZERO 0
// 入出力系
#define LEFT_MOTER OUT_C
#define RIGHT_MOTER OUT_A
#define LIGHT_SENSOR S3
/* 直角の制御 */
#define TURN_COUNT_MAX 300
#define GO_COUNT_MAX 150
// スピード
#define SPEED 40
/* アームの制御 */
#define BALL_DISTANCE 9
#define BALLBAR_DOWN 100
#define BALLBAR_UP 1000
#define BALLBAR_WAIT 1000
#define BALLBAR_SPEED 50
**関数の定義 [#fabca395]
コードの簡略化のための関数です。
/*
* 左右のモーターを回す
* スピードが正のときは前、負のときは後に回す
// 回転センサーの係数
#define MOTER_ROTATE (1 / 14.55)
// 車体の幅
#define BODY_WIDTH 12.2
// 回転センサーの値を取得する
#define LeftRotation() MotorRotationCount(OUT_C)
#define RightRotation() MotorRotationCount(OUT_A)
/*
* モーターを回転させる
* スピードが負のときは逆回転
* 0のときは停止
*/
void On(int leftSpeed, int rightSpeed){
if(leftSpeed > 0){
OnFwd(OUT_C, leftSpeed);
OnFwd(LEFT_MOTER, leftSpeed);
}else if(leftSpeed){
OnRev(LEFT_MOTER, -leftSpeed);
}else{
OnRev(OUT_C, -leftSpeed);
Off(LEFT_MOTER);
}
if(rightSpeed > 0){
OnFwd(OUT_A, rightSpeed);
OnFwd(RIGHT_MOTER, rightSpeed);
}else if(rightSpeed){
OnRev(RIGHT_MOTER, -rightSpeed);
}else{
OnRev(OUT_A, -rightSpeed);
Off(RIGHT_MOTER);
}
}
/* 赤外線センサーの値を5段階(0〜4)で返す */
/*
* 明るさを 0 〜 4 で返す
* 暗い < 0 1 2 3 4 > 明るい
*/
bool Light(){
if(SENSOR_3 < 40){
return 0;
}else if(SENSOR_3 < 44){
}else if(SENSOR_3 < 45){
return 1;
}else if(SENSOR_3 < 48){
}else if(SENSOR_3 < 50){
return 2;
}else if(SENSOR_3 < 52){
}else if(SENSOR_3 < 55){
return 3;
}else{
return 4;
}
}
/* 止まる */
void Stay(){
Off(OUT_AC);
}
/* 前へ進む */
void GoForward(){
On(SPEED_HIGH, SPEED_HIGH);
}
/* 左へ進む */
void GoLeft(){
On(SPEED_ZERO, SPEED_MEDIUM);
}
/* 右へ進む */
void GoRight(){
On(SPEED_MEDIUM, SPEED_ZERO);
}
/* 左回転 */
void TurnLeft(){
On(-SPEED_LOW, SPEED_LOW);
}
/* 右回転 */
void TurnRight(){
On(SPEED_LOW, -SPEED_LOW);
}
/* 後ろへ戻る */
void Back(){
On(-SPEED_HIGH, -SPEED_HIGH);
}
/* 左後へ戻る */
void BackLeft(){
On(SPEED_ZERO, -SPEED_MEDIUM);
}
/* 右後へ戻る */
void BackRight(){
On(-SPEED_MEDIUM, SPEED_ZERO);
}
/* ボールを見つけるとtrueを返す */
bool checkBall(){
if(SensorUS(S1) <= BALL_DISTANCE){
return true;
}
return false;
}
/* ボールを拾う */
void catchBall(){
OnFwd(OUT_B, BALLBAR_SPEED);
Wait(BALLBAR_DOWN);
Float(OUT_B);
Wait(BALLBAR_WAIT);
}
/* ボールを離す */
void releaseBall(){
OnRev(OUT_B, BALLBAR_SPEED);
Wait(BALLBAR_UP);
Float(OUT_B);
Wait(BALLBAR_WAIT);
}
**メイン関数 [#j9bbb92a]
task main(){
/* 回転数を記録するための変数 */
int turnCount = 0;
/* 曲がりながら進んだ回数を記録するための変数 */
int goCount = 0;
/* 直角の回数 */
int nCorner = 0;
/* ボールを認識した回数 */
int ballCount = 0;
/* センサー類を初期化 */
SetSensorLowspeed(S1);
SetSensorLight(S3);
while(nCorner < 5){
//角の回数を数えるための変数
int corner = 0;
// 車体の向き
float rotation = 0;
// 光センサー初期化
SetSensorLight(LIGHT_SENSOR);
// スタートから出る
On(SPEED, SPEED);
until(Light() == 0);
until(Light() == 4);
// トレース開始
while(corner < 8){
Wait(1);
/* ボールを拾う処理 */
/* 直角1回目と2回目の間で、まだボールを拾っていない場合 */
if(nCorner == 1 && ballCount >= 0){
/* ボールを認識したら */
if(checkBall()){
PlayTone(880, 100);
/* 1秒間ボールを認識すれば */
if(ballCount++ > 10){
/* ボールを拾う */
catchBall();
ballCount = -1;
}
/* ボールを認識している間はトレースを一時停止 */
Stay();
Wait(99);
continue;
}else{
ballCount = 0;
}
}
/* トレース */
switch(Light()){
/* 明るさレベル0のとき */
case 0:
TurnLeft();
if(turnCount < 0 || goCount < 0){
turnCount = 0;
goCount = 0;
case 0: //左回転
On(-SPEED, SPEED);
break;
case 1: //左折
On(0, SPEED);
break;
case 2: //直進
On(SPEED, SPEED);
break;
case 3: //右折
On(SPEED, 0);
break;
case 4: //右回転
On(SPEED, -SPEED);
break;
}
// 回転センサーの値を取得する
int _tl = LeftRotation();
int _tr = RightRotation();
// 差から角度を求める
rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI);
tl = _tl;
tr = _tr;
// 通過ポイント
switch(corner){
case 0: // 1回目のヘアピンカーブ
if(rotation > 90){
PlayTone(TONE_C4, 100);
corner++;
}
turnCount++;
break;
/* 明るさレベル1のとき */
case 1:
GoLeft();
if(turnCount < 0 || goCount < 0){
turnCount = 0;
goCount = 0;
case 1: // 1回目の交差点
if(rotation < 20){
PlayTone(TONE_D4, 100);
On(SPEED, SPEED);
until(Light() == 0);
until(Light() >= 2);
corner++;
}
goCount++;
break;
/* 明るさレベル2のとき */
case 2:
GoForward();
case 2: // ボールを取る位置
if(rotation < -40){
PlayTone(TONE_E4, 100);
Wait(500);
On(SPEED, SPEED);
RotateMotor(OUT_B, 10, 90);
On(-SPEED, -SPEED);
Wait(500);
On(SPEED, 0);
until(Light() < 4);
corner++;
}
break;
/* 明るさレベル3のとき */
case 3:
GoRight();
if(turnCount > 0 || goCount > 0){
turnCount = 0;
goCount = 0;
case 3: // 大きいカーブ終了
if(rotation < -180){
PlayTone(TONE_F4, 100);
corner++;
}
goCount++;
break;
/* 明るさレベル4のとき */
case 4:
TurnRight();
if(turnCount > 0 || goCount > 0){
turnCount = 0;
goCount = 0;
case 4: // 2回目の交差点
if(rotation > -180){
PlayTone(TONE_G4, 100);
On(SPEED, 0);
until(Light() == 4);
On(SPEED, SPEED);
until(Light() == 3);
corner++;
}
turnCount++;
break;
case 5: // 3回目の交差点
if(rotation > 10){
PlayTone(TONE_A4, 100);
On(SPEED, 0);
until(Light() >= 3);
corner++;
}
break;
case 6: // 2回目のヘアピンカーブ
if(rotation > 90){
PlayTone(TONE_B4, 100);
corner++;
}
break;
case 7: // ゴール
if(rotation < -15){
PlayTone(TONE_C5, 100);
On(0, 0);
RotateMotor(OUT_B, 100, -90);
corner++;
}
break;
}
/* 直角判定 */
/* もし回転数が多く直進回数が少なければ */
if(turnCount > TURN_COUNT_MAX && goCount < GO_COUNT_MAX){
PlayTone(440, 100);
switch(nCorner++){
/* 1,2,4回目の直角のとき */
case 0:
case 1:
case 3:
/* 交差点を渡る */
TurnRight();
Wait(turnCount * 3);
Off(OUT_AC);
Wait(100);
GoForward();
Wait(100);
break;
/* 5回目の直角のとき */
case 4:
/* 向きを訂正して */
/* ボールを離す */
break;
}
turnCount = 0;
goCount = 0;
}
}
}
Wait(2000);
}
**解説 [#k0f87237]
***トレース [#f99fefa4]
明るさを0から4の5段階に分けて、0と4のときはその場で回転、1と3のときは曲がりながら進む、2のときは直進するようします。
モーターの回転センサーを使って車体の向きを計算します。(雑に計算していますが大体あっているようなので修正しませんでした。)
いくつかのポイントを設定して、コースのどのあたりにいるかを求めます。
交差点を超えるために直角の回数を数えます。
スタート→ゴールの場合は1,2,4回目の直角は交差点、3回目は直角コーナー、5回目はゴールとなります。
交差点と判断すると向きを戻して直進して交差点を渡ります。
直角コーナーはそのままで曲がってくれるのでプログラムは不要です。
ゴールの場合は向きを戻し、ボールをシュートしてループを抜けます。
***ボール [#ec9fadc9]
超音波センサーがボールを見つけるとアームを下ろします。
ただし、超音波センサーの誤動作も考えられるので、ボールを見つけたときにはトレースを中断して、1秒間連続で認識し続けたらボールがあると認識するようにしました。
1つ目の交差点を過ぎるとアームを下ろします、このときボールが坂をのぼります。
シュートはアームを上げるとボールが坂を転がります。
*感想 [#n5075af2]
なかなか思っている通りに動かず苦労しました。特に値の調整が大変でした。