- 追加された行はこの色です。
- 削除された行はこの色です。
[[2014a/Member]]
*課題の説明 [#r13fce05]
詳しくは[[2014a/Mission2]]を見てください。
*メンバー [#na188e61]
tadanobo
mame
dai
bgpad
*ロボット本体 [#bdd03821]
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,70%);S__2424847.jpg
**アーム [#y6f68c61]
&ref(2014a/Member/mame/Mission2/ file写真 2014-07-25 16 36 37(1).jpg ,70%);
&ref(2014a/Member/mame/Mission2/ file写真 2014-07-25 16 36 37(1).jpg ,70%);
**超音波センサー [#t96c02d0]
*作戦 [#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]