[[2014a/Member]] *課題の説明 [#r13fce05] *メンバー [#na188e61] tadanobo mame dai bgpad *ロボット本体 [#bdd03821] *作戦 [#b8f08a37] *プログラム [#z80241ef] 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 //コップ1のしきい値 #define CUP2 30 //コップ2のしきい値 #define CUP3 20 //コップ3のしきい値 #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){ //コップ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); } *反省と感想 [#dc1c78a7]