2014a/Member/dai/Mission2 の履歴差分(No.3) - ロボティクス入門ゼミ

履歴一覧
現在との差分を表示
ソースを表示
2014a/Member/dai/Mission2 へ行く。
- 1 (2014-08-08 (金) 12:13:26)
- 2 (2014-08-08 (金) 14:36:59)
- 3 (2014-08-09 (土) 15:05:54)
- 4 (2014-08-10 (日) 03:23:04)
追加された行はこの色です。
削除された行はこの色です。
[[2014a/Member]]

目次

#contents

*課題の説明 [#sa503f4f]

課題１で使用したコースを２枚用いて課題２を行う。１～３と書かれた紙コップを２組作り、ランダムに配置された１組の紙コップを運び、もう１組の紙コップに重ねる。紙コップは色を塗ることで光センサーで識別できるようにする。

詳しい説明は[[2014a/Mission2]]を見てください。

*メンバー [#i813c647]

・tadanobo

・mame

・dai

・bgpat

*ロボット本体 [#d1181d24]

&ref(2014a/Member/dai/Mission1/写真 2014-07-25 16 36 37.jpg,30%);
&ref(2014a/Member/dai/Mission2/写真 2014-07-25 16 36 37.jpg,15%);

ロボットの本体を２つ使用し、bluetoothを用いて連動させる。マスターの機器で走行とセンサーをコントロールし、スレーブの機器でアームの制御を行う。超音波センサーで紙コップを探し、アームを降ろし、掴み、持ち上げる。

&ref(2014a/Member/dai/Mission2/写真 2014-08-07 16 24 54.jpg,15%);

紙コップを落とさないために、持ち上げた後、持ち直す。そのとき、光センサーで紙コップの色を識別する。

&ref(2014a/Member/dai/Mission2/写真 2014-08-07 16 25 37.jpg,15%);

後輪の開発に苦労した。丈夫でかつ、滑りやすくするために工夫した。

* プログラム[#zec3fe48]

 long timerStarted = 0; //タイマーをセット

 void StartTimer(){
   timerStarted = CurrentTick(); //時間を数え始める 
 }

 long StopTimer(){
   return CurrentTick() - timerStarted;
 }

 void Debug(int line, string text){
   TextOut(0, (8 - line) * 8, StrCat(text, "                    "));
 }

 void DebugText(int line, string name, string value){
   Debug(line, StrCat(name, ": ", value));
 }

 void DebugNumber(int line, string name, int num){
   DebugText(line, name, NumToStr(num));
 }

 void DebugSound(int freq){
   PlayTone(freq, 100);
 }

 #define LEFT_WHEEL OUT_C //左のタイヤのモーター
 #define RIGHT_WHEEL OUT_A //右のタイヤのモーター
 #define BOTH_WHEEL OUT_AC //両タイヤのモーター
 
 #define ARM_BASE OUT_A //本体側のアームのモーター
 #define ARM_HAND OUT_B //アーム先のモーター
 
 #define TRACE_SENSOR S1 //トレース用光センサー
 #define DIST_SENSOR S2 //距離測定用超音波センサー
 #define TYPE_SENSOR S3 //紙コップ判別用光センサー

 void InitSensor(){
   SetSensorLight(TRACE_SENSOR);
   SetSensorLowspeed(DIST_SENSOR);
   SetSensorLight(TYPE_SENSOR);
 }

 #define BODY_WIDTH 12.6
 #define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
 
 #define DIRECTION_DEBUG 1

 float direction = 0;
 float leftMoterCount = 0;
 float rightMoterCount = 0;
 float directionTimer = 0;

 float GetDirection(){
   float out = MotorRotationCount(RIGHT_WHEEL);
   float in = MotorRotationCount(LEFT_WHEEL);
   direction += (out - leftMoterCount - in + rightMoterCount) * DIRECTION_COE;
   leftMoterCount = out;
   rightMoterCount = in;
   DebugNumber(DIRECTION_DEBUG, "Direction", direction);
   return direction;
 }

 void StartDirectionTimer(){
   directionTimer = GetDirection();
 }

 float StopDirectionTimer(){
   return GetDirection() - directionTimer;
 }

 #define THRESHOLD 43
 
 #define SPEED_FORWARD 70
 #define SPEED_TURN 80
 #define SPEED_SPIN 60
 #define SPEED_FORWARD_LOW 25
 #define SPEED_SPIN_LOW 40

 #define TraceSensor SENSOR_1

 #define DEBUG_TRACETYPE 3
 #define DEBUG_TRACESENSOR 2
 
 #define WHEEL_STRAIGHT (80 / 1715)

 int lastTrace = -1;
 int forwardCount = 0;
 float rightDirection = 0;

 void Stay(){
   Off(BOTH_WHEEL);
   DebugText(DEBUG_TRACETYPE, "State", "Stay");
 }

 void GoForward(){
   OnFwdSync(BOTH_WHEEL, SPEED_FORWARD, 0);
   DebugText(DEBUG_TRACETYPE, "State", "GoForward");
 }

 void GoBack(){
   OnRevSync(BOTH_WHEEL, SPEED_FORWARD, 0);
   DebugText(DEBUG_TRACETYPE, "State", "GoBack");
 }

 void SpinLeft(){
   OnFwdSync(BOTH_WHEEL, SPEED_SPIN, -100);
   DebugText(DEBUG_TRACETYPE, "State", "SpinLeft");
 } 

 void SpinRight(){
   OnFwdSync(BOTH_WHEEL, SPEED_SPIN, 100);
   DebugText(DEBUG_TRACETYPE, "State", "SpinRight");
 }

 void GoForwardLow(){
   OnFwdSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
   DebugText(DEBUG_TRACETYPE, "State", "GoForwardLow");
 } 

 void GoBackLow(){
   OnRevSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
   DebugText(DEBUG_TRACETYPE, "State", "GoBackLow");
 }

 void SpinLeftLow(){
   OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, -100);
   DebugText(DEBUG_TRACETYPE, "State", "SpinLeftLow");
 }

 void SpinRightLow(){
   OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, 100);
   DebugText(DEBUG_TRACETYPE, "State", "SpinRightLow");
 }

 void TurnLeft(){
   Stay();
   OnFwd(RIGHT_WHEEL, SPEED_TURN);
   DebugText(DEBUG_TRACETYPE, "State", "TurnLeft");
 } 
 
 void TurnRight(){
   Stay();
   OnFwd(LEFT_WHEEL, SPEED_TURN);
   DebugText(DEBUG_TRACETYPE, "State", "TurnRight");
 }

 void UntilBlack(){
   until(TraceSensor < THRESHOLD);
 }
 
 void UntilWhite(){
   until(TraceSensor > THRESHOLD);
 }
 
 void UntilLine(){
   UntilBlack();
   UntilWhite();
 }
 
 int Trace(){
   int n;
   float d;
   if(TraceSensor < THRESHOLD - 8){
       SpinLeft();
       n = 0;
   }else if(TraceSensor < THRESHOLD - 2){
       TurnLeft();
       n = 1;
   }else if(TraceSensor < THRESHOLD + 2){
       GoForward();
       n = 2;
   }else if(TraceSensor < THRESHOLD + 8){
       TurnRight();
       n = 3;
   }else{
       SpinRight();
       d = GetDirection();
       if(lastTrace == 4){
           if(d - rightDirection < -360){
               PlayTone(440, 100);
               GoBack();
               UntilBlack();
           }
       }else{
           rightDirection = d;
       }
       n = 4;
   }
   DebugNumber(DEBUG_TRACESENSOR, "State", TraceSensor);
   if(n == 2){
       forwardCount++;
   }else{
       forwardCount = 0;
   }
   lastTrace = n;
   return n;
 }

 void Spin(int degree){
   if(degree > 0){
       StartDirectionTimer();
       SpinLeft();
       until(StopDirectionTimer() >= degree);
   }else{
       StartDirectionTimer();
       SpinRight();
       until(StopDirectionTimer() <= degree);
   }
 }

 void SpinLow(int degree){
   if(degree > 0){
       StartDirectionTimer();
       SpinLeftLow();
       until(StopDirectionTimer() >= degree);
   }else{
       StartDirectionTimer();
       SpinRightLow();
       until(StopDirectionTimer() <= degree);
   }
 } 

 void GoForwardTo(int x){
   float a = MotorRotationCount(LEFT_WHEEL);
   GoForward();
   until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
   Stay();
 }

 void GoForwardToLow(int x){
   float a = MotorRotationCount(LEFT_WHEEL);
   GoForwardLow();
   until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
   Stay();
 }

 void GoBackTo(int x){
   float a = MotorRotationCount(LEFT_WHEEL);
   GoBack();
   until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
   Stay();
 }

 void GoBackToLow(int x){
   float a = MotorRotationCount(LEFT_WHEEL);
   GoBackLow();
   until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
   Stay();
 }

 #define DIST_CATCH 13
 #define DIST_STACK 13
 
 #define DistSensor SensorUS(DIST_SENSOR)

 void TowardCup(int degree){
   int min = DistSensor;
   float right = GetDirection();
   SpinLow(-degree * 0.5);
   StartDirectionTimer();
   SpinLeftLow();
   while(StopDirectionTimer() < degree){
       if(DistSensor < min){
           min = DistSensor;
           right = GetDirection();
       }
   }
   SpinRightLow();
   until(GetDirection() <= right);
   Stay();
 }

 int PrepareCatch(){
   int l = 0;
   TowardCup(90);
   GoForwardToLow(DistSensor - 5);
   l += DistSensor - 5;
   Stay();
   TowardCup(120);
   GoBackToLow(DIST_CATCH - DistSensor);
   l += DistSensor - DIST_CATCH;
   Stay();
   return l * WHEEL_STRAIGHT;
 } 

 int PrepareStack(){
   int l = 0;
   TowardCup(90);
   GoForwardToLow(DistSensor - 5);
   l += DistSensor - 5;
   Stay();
   TowardCup(120);
   GoBackToLow(DIST_STACK - DistSensor);
   l += DistSensor - DIST_STACK;
   Stay();
   return l * WHEEL_STRAIGHT;
 }

 #define DOWN_POWER 30
 #define UP_POWER 90
 #define POWER 20 

 #define ARM1_DEGREE 90
 #define ARM2_DEGREE 210

 #define CUP1 45 //コップ１のしきい値
 #define CUP2 30 //コップ２のしきい値
 #define CUP3 20 //コップ３のしきい値

 #define TypeSensor SENSOR_3

 void InitArm(){
   SetSensorLight(DIST_SENSOR);
 }

 void DownArm(){
   Off(ARM_BASE);
   Wait(100);
   RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
   OnFwd(ARM_BASE, 50);
   Wait(1000);
 }

 void UpArm(){
   Off(ARM_BASE);
   Wait(100);
   RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE);
   OnRev(ARM_BASE, 50);
   Wait(2000);
 }

 void HoldCup(){
   OnFwd(ARM_HAND, POWER);
   int s = 0;
   while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE){
       if(DistSensor > s){
           s = DistSensor;
       }
   };
 } 

 void ReleaseCup(){
   RotateMotor(ARM_HAND, POWER, -ARM2_DEGREE);
 }

 void CatchCup(){
   HoldCup();
   Wait(500);
   UpArm();
   Wait(500);
   ReleaseCup();
   HoldCup();
 }

 void StackCup(){
   DownArm();
   Wait(500);
   ReleaseCup();
   Wait(500);
 }

 int GetCupType(){
   int n = 0;
   DebugNumber(2, "sensor", TypeSensor);
   if(TypeSensor > CUP1){
       n = 1;
       DebugSound(TONE_C4);
   }else if(TypeSensor > CUP2){
       n = 2;
       DebugSound(TONE_D4);
   }else if(TypeSensor > CUP3){
       n = 3;
       DebugSound(TONE_E4);
   }
   return n;
 }

 #define BT_CONN 1
 #define BT_MAILBOX MAILBOX3
 
 #define BT_NULL 0
 #define BT_CATCH 1
 #define BT_STACK 2
 #define BT_FINISH 4

 void BTStart(string file){
   until(BluetoothStatus(BT_CONN) == NO_ERR);
   RemoteStartProgram(BT_CONN, file);
 }

 int BTReceiveMessage(){
   int msg = BT_NULL;
   while(msg == BT_NULL){
       ReceiveRemoteNumber(BT_MAILBOX, true, msg);
       DebugNumber(5, "debug", msg);
   }
   return msg;
 }

 void BTSendMessage(int msg, bool isMaster){
   if(isMaster){
       SendRemoteNumber(BT_CONN, BT_MAILBOX, msg);
   }else{
       SendResponseNumber(BT_MAILBOX, msg);
   }
 }

 void GoCup1(){
   InitSensor();
   Spin(-90);
   Stay();Wait(100);
   GoForwardTo(60);
   Stay();Wait(100);
   Spin(135);
   Stay();Wait(100);
   GoForwardTo(10);
   Stay();Wait(100);
 }

 void BackCup1(int l){
   GoBackTo(10 + l);
   Stay();Wait(100);
   Spin(-direction - 90);
   Stay();Wait(100);
   GoBackTo(50);
   Stay();Wait(100);
   Spin(90);
 }

 void GoCup2(){
   Spin(-90);
   Stay();Wait(100);
   GoForwardTo(60);
   Stay();Wait(100);
   Spin(45);
   Stay();Wait(100);
 }

 void BackCup2(int l){
   GoBackTo(10 + l); //@
   Stay();Wait(100);
   Spin(-direction - 90);
   Stay();Wait(100);
   GoBackTo(50); //@
   Stay();Wait(100);
   Spin(90); 
 }

 void GoCup3(){
   InitSensor();
   Spin(-90);
   Stay();Wait(100);
   GoForwardTo(60);
   Stay();Wait(100);
   Spin(90);
   Stay();Wait(100);
   GoForwardTo(30);
   Stay();Wait(100);
 }

 void BackCup3(){
   GoBackTo(35); //@
   Stay();Wait(100);
   Spin(-direction - 100); //@
   Stay();Wait(100);
   GoBackTo(60);
   Stay();Wait(100);
   Spin(90);
 }
 
 task main(){
   int l;
   BTStart("slave3.rxe");
   InitSensor();
   GoCup1();
   l = PrepareCatch();
   //MasterCatchCup();
   BTSendMessage(BT_CATCH, true);
   BTReceiveMessage();
   BackCup1(l);
   int type = GetCupType();
   if(type == 1){  //コップ１の場合
       Spin(45); //45度回転
       GoForwardTo(15);
   }else if(type == 2){
       Spin(-45);
       GoForwardTo(15);
   }else{
       GoForwardTo(30);
   }
   PrepareStack();
   //MasterCatchCup();
   BTSendMessage(BT_STACK, true);
   BTReceiveMessage();
   Stay();Wait(100);
   if(type == 1){
       Spin(45 - GetDirection());
       GoBackTo(15);
   }else if(type == 2){
       Spin(-45 - GetDirection());
       GoBackTo(15);
   }else{
       Spin(-GetDirection());
       GoBackTo(30);
   }
   Spin(-GetDirection());
   Stay();Wait(100);
   
   GoCup2();
   l = PrepareCatch();
   //MasterCatchCup();
   BTSendMessage(BT_CATCH, true);
   BTReceiveMessage();
   BackCup2(l);
   type = GetCupType();
   if(type == 1){
       Spin(45);
       GoForwardTo(15);
   }else if(type == 2){
       Spin(-45);
       GoForwardTo(15);
   }else{
       GoForwardTo(30);
   }
   PrepareStack();
   //MasterCatchCup();
   BTSendMessage(BT_STACK, true);
   BTReceiveMessage();
   Stay();Wait(100);
   if(type == 1){
       Spin(45 - GetDirection());
       GoBackTo(15);
   }else if(type == 2){
       Spin(-45 - GetDirection());
       GoBackTo(15);
   }else{
       Spin(-GetDirection());
       GoBackTo(30);
   }
   Spin(-GetDirection());
   Stay();Wait(100);
   
   GoCup3();
   PrepareCatch();
   //MasterCatchCup();
   BTSendMessage(BT_CATCH, true);
   BTReceiveMessage();
   BackCup3();
   type = GetCupType();
   if(type == 1){
       Spin(45);
       GoForwardTo(15);
   }else if(type == 2){
       Spin(-45);
       GoForwardTo(15);
   }else{
       GoForwardTo(30);
   }
   PrepareStack();
   //MasterCatchCup();
   BTSendMessage(BT_STACK, true);
   BTReceiveMessage();
   Stay();Wait(100);
   if(type == 1){
       Spin(45 - GetDirection());
       GoBackTo(15);
   }else if(type == 2){
       Spin(-45 - GetDirection());
       GoBackTo(15);
   }else{
       Spin(-GetDirection());
       GoBackTo(30);
   }
   Spin(-GetDirection());
   Stay();Wait(100);
   
   BTSendMessage(BT_FINISH, true);
 }

* 反省・感想[#ja007ef1]

・課題１に比べて、複雑なプログラミングが多く、製作に苦労した。

・より確実に紙コップを掴むため、アームの部分の部分の製作に時間がかかった。

・後輪の性能を高めるために、試行錯誤を繰り返した。

・プログラミングを部分的にテストすることで短時間での改善ができるようにした。

・プログラミングを主体として作ってくれた仲間に感謝したい。