- Äɲ䵤줿¹Ô¤Ï¤³¤Î¿§¤Ç¤¹¡£
- ºï½ü¤µ¤ì¤¿¹Ô¤Ï¤³¤Î¿§¤Ç¤¹¡£
[[2014a/Member]]
*²ÝÂê¤ÎÀâÌÀ [#r13fce05]
¾Ü¤·¤¯¤Ï[[2014a/Mission2]]¤ò¸«¤Æ¤¯¤À¤µ¤¤¡£
*¥á¥ó¥Ð¡¼ [#na188e61]
tadanobo
mame
dai
bgpad
*¥í¥Ü¥Ã¥ÈËÜÂÎ [#bdd03821]
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,70%);S__2424847.jpg
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%);
**¥¢¡¼¥à [#y6f68c61]
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-07-25 16 36 37(1).jpg,70%);
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-07-25 16 36 37(1).jpg,25%);
¡¥³¥Ã¥×¤ò¤Ä¤«¤à
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 24 50.jpg,8%);
¢¥¢¡¼¥à¤¬²¼¤ê¤ë
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
£Â¾¤Î¥³¥Ã¥×¤Ë¤Ï¤á¤ë
**Ķ²»ÇÈ¥»¥ó¥µ¡¼ [#t96c02d0]
**¥É¥é¥¤¥Ö¥Ù¡¼¥¹ [#je9abe0b]
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 25 32.jpg,30%);
*ºîÀï [#b8f08a37]
*¥×¥í¥°¥é¥à [#z80241ef]
long timerStarted = 0; //¥¿¥¤¥Þ¡¼¤ò¥»¥Ã¥È
#define MAX(a, b) (a > b ? a : b)
#define MIN(a, b) (a < b ? a : b)
#define ABS(a) (a < 0 ? -a : a)
}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);
}
*È¿¾Ê¤È´¶ÁÛ [#dc1c78a7]
À®¸ùΨ¤ò¾å¤²¤ë¤¿¤á¤Ë²¿ÅÙ¤âÄ´À°¤·Ä¾¤·¤¿¡£