2014a/Member/mame/Mission2
¤ò¥Æ¥ó¥×¥ì¡¼¥È¤Ë¤·¤ÆºîÀ®
[
¥È¥Ã¥×
] [
¿·µ¬
|
°ìÍ÷
|
¸¡º÷
|
ºÇ½ª¹¹¿·
|
¥Ø¥ë¥×
|
¥í¥°¥¤¥ó
]
³«»Ï¹Ô:
[[2014a/Member]]
Ìܼ¡
#contents
*²ÝÂê¤ÎÀâÌÀ [#r13fce05]
¾Ü¤·¤¯¤Ï[[2014a/Mission2]]¤ò¸«¤Æ¤¯¤À¤µ¤¤¡£
*¥á¥ó¥Ð¡¼ [#na188e61]
tadanobo
mame
dai
bgpad
*¥í¥Ü¥Ã¥ÈËÜÂÎ [#bdd03821]
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%);
¥¢¡¼¥à¡¢Æ¹ÂΡ¢¥É¥é¥¤¥Ö¥Ù¡¼¥¹¤È¡¢¥Ñ¡¼¥Ä¤´¤È¤Ëʬ¤±¤¿¤Î¤ÇÁȤßΩ¤Æ¡¢¼è¤ê³°¤·¤¬ÍưפǤ¢¤ë
**¥¢¡¼¥à [#y6f68c61]
&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,7%);
¢¥¢¡¼¥à¤¬²¼¤ê¤ë
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
£Â¾¤Î¥³¥Ã¥×¤Ë½Å¤Í¤ë
**¥É¥é¥¤¥Ö¥Ù¡¼¥¹ [#je9abe0b]
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 25 32.jpg,10%);
µ¡ÂΤ¬½Å¤¤¤Î¤Ç¥Ù¡¼¥¹¤Ï´è¾æ¤Ë¤·¤¿¡£¤Þ¤¿¡¢¸åÎؤϥ¹¥à¡¼¥º¤ËÆ°¤¯¤è¤¦¤ËÂ礤ʤâ¤Î¤Ë¤·¤¿
*ºîÀï [#b8f08a37]
¥³¥Ã¥×¤ò³Î¼Â¤Ë¤È¤ë¤¿¤á¤ËÆó²ó¥³¥Ã¥×¤òȽÃǤ¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£
Ķ²»ÇÈ¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤Î°ÌÃÖ¤ò³Îǧ¤·¡¢¸÷¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤ÎÈÖ¹æ¤ò¸«Ê¬¤±¤ë¤è¤¦¤Ë¤·¤¿¡£
¤Þ¤¿¡¢¾¯¤·¤Ç¤â³ÑÅÙ¤¬¤º¤ì¤Ê¤¤¤è¤¦¤Ë¥³¥Ã¥×¤ò¤È¤ëËè¤Ë³ÑÅÙ¤ò½¤Àµ¤¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£
¼ç¤Ë¹ÔÆ°¤¹¤ë¤¿¤á¤Î¿Æµ¡¡¢¼ç¤Ë¥³¥Ã¥×¤ò¤È¤ë¤¿¤á¤Î»Òµ¡¤Ë¥×¥í¥°¥é¥à¤òʬ¤±¤¿¡£
*¥×¥í¥°¥é¥à [#z80241ef]
**´ðËÜ¤Î¥×¥í¥°¥é¥à [#yb10803f]
***ÄêµÁ [#ua28c278]
#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 DIST_SENSOR S2 //µ÷Υ¬ÄêÍÑĶ²»ÇÈ¥»¥ó¥µ¡¼
#define TYPE_SENSOR S3 //»æ¥³¥Ã¥×ȽÊÌÍѸ÷¥»¥ó¥µ¡¼
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#k87d7c7e]
void InitSensor(){
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
}
**µ¡ÂΤιÔÆ°¤¹¤ë¥×¥í¥°¥é¥à [#edcd34a9]
***ÄêµÁ [#w23ba2df]
#define SPEED_STRAIGHT 50
#define SPEED_SPIN 40
#define SPEED_STRAIGHT_LOW 20
#define SPEED_SPIN_LOW 30
#define STAY_WAIT 50
#define WHEEL_STRAIGHT (80 / 1715)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#h5000208]
void DebugDriveType(string type, int speed){
DebugNumber(DEBUG_DRIVETYPE, type, speed);
}
void Stay(){
Off(BOTH_WHEEL);
DebugDriveType("Stay", 0);
}
void StayWait(){
Stay();
Wait(STAY_WAIT);
}
void Drive(int speed, int rate, string type){
if(speed){
if(speed > 0){
OnFwdSync(BOTH_WHEEL, speed, rate);
}else{
OnRevSync(BOTH_WHEEL, -speed, rate);
}
DebugDriveType(type, speed);
}else{
}
}
void DriveStraight(int speed){
Drive(speed, 0, "Straight");
}
void DriveLeft(int speed){
Drive(speed, -100, "Left");
}
void DriveRight(int speed){
Drive(speed, 100, "Right");
}
void Forward(){
DriveStraight(SPEED_STRAIGHT);
}
void ForwardLow(){
DriveStraight(SPEED_STRAIGHT_LOW);
}
void Back(){
DriveStraight(-SPEED_STRAIGHT);
}
void BackLow(){
DriveStraight(-SPEED_STRAIGHT_LOW);
}
void SpinLeft(){
DriveLeft(SPEED_SPIN);
}
void SpinLeftLow(){
DriveLeft(SPEED_SPIN_LOW);
}
void SpinRight(){
DriveRight(SPEED_SPIN);
}
void SpinRightLow(){
DriveRight(SPEED_SPIN_LOW);
}
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 GoForward(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Forward();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("Forward", d);
}
Stay();
}
void GoForwardLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
ForwardLow();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("ForwardLow", d);
}
Stay();
}
void GoBack(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Back();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("Back", d);
}
Stay();
}
void GoBackLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
BackLow();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("BackLow", d);
}
Stay();
}
void GoStraight(int x){
if(x > 0){
GoForward(x);
}else{
GoBack(-x);
}
}
void GoStraightLow(int x){
if(x > 0){
GoForwardLow(x);
}else{
GoBackLow(-x);
}
}
**¥³¥Ã¥×¤òõ¤¹¤¿¤á¤Î¥×¥í¥°¥é¥à [#w225d5ad]
***ÄêµÁ [#p8f79d0c]
#define DIST_CATCH 12
#define DIST_STACK 11
#define DIST_SEARCH 6
#define SEARCH_DIRECTION1 90
#define SEARCH_DIRECTION2 120
#define DistSensor SensorUS(DIST_SENSOR)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#ie6d8cf2]
void TowardCup(int degree){
int min = DistSensor;
float dir = GetDirection();
SpinLow(-degree * 0.5);
StartDirectionTimer();
SpinLeftLow();
while(StopDirectionTimer() < degree){
if(DistSensor < min){
min = DistSensor;
dir = GetDirection();
}
}
SpinRightLow();
until(GetDirection() <= dir);
Stay();
}
int PrepareCup(int dist){
int x, y;
TowardCup(SEARCH_DIRECTION1);
x = DistSensor - DIST_SEARCH;
GoForwardLow(x);
StayWait();
TowardCup(SEARCH_DIRECTION2);
y = dist - DistSensor;
GoBackLow(y);
StayWait();
return x - y;
}
int PrepareCatch(){
return PrepareCup(DIST_CATCH);
}
int PrepareStack(){
return PrepareCup(DIST_STACK);
}
**¥³¥Ã¥×¤òȽÃǤ¹¤ë¤¿¤á¤Î¥×¥í¥°¥é¥à [#h13b91a8]
***ÄêµÁ [#uac0f5e7]
#define DIRECTION_DEBUG 1
#define DEBUG_DRIVETYPE 2
#define DEBUG_CUPTYPE 3¡¡
#define DEBUG_CUPSENSOR 3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#c68e677d]
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);
}
**Bluetooth¤Î¥×¥í¥°¥é¥à [#e5c11d2e]
***ÄêµÁ [#pea36e5f]
#define BT_CONN 1
#define BT_MAILBOX MAILBOX3
#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#m0f1915a]
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 BTStop(){
BTSendMessage(BT_FINISH, true);
}
**¥¢¡¼¥à¤Î¥×¥í¥°¥é¥à [#n48ddf38]
***ÄêµÁ [#x6c230d3]
#define DOWN_POWER 30
#define HOLD_DOWN_POWER 50
#define UP_POWER 90
#define HOLD_UP_POWER 50
#define HOLD_POWER 20
#define RELEASE_POWER 80
#define ARM1_DEGREE 90
#define ARM2_DEGREE 210
#define CUP1 55
#define CUP2 37
#define CUP3 23
#define TypeSensor SENSOR_3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#o58a78f8]
void DownArm(){
Off(ARM_BASE);
Wait(100);
RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
OnFwd(ARM_BASE, HOLD_DOWN_POWER);
Wait(1000);
}
void UpArm(){
Off(ARM_BASE);
Wait(100);
RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE);
OnRev(ARM_BASE, HOLD_UP_POWER);
Wait(2000);
}
void HoldCup(){
OnFwd(ARM_HAND, HOLD_POWER);
while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE);
}
void ReleaseCup(){
RotateMotor(ARM_HAND, RELEASE_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;
if(TypeSensor > (CUP1 + CUP2) * 0.5){
n = 1;
}else if(TypeSensor > (CUP2 + CUP3) * 0.5){
n = 2;
}else{
n = 3;
}
DebugNumber(DEBUG_CUPTYPE, "CupNumber", n);
DebugNumber(DEBUG_CUPSENSOR, "CupSensor", TypeSensor);
return n;
}
void MasterCatchCup(){
BTSendMessage(BT_CATCH, true);
BTReceiveMessage();
}
void MasterStackCup(){
BTSendMessage(BT_STACK, true);
BTReceiveMessage();
}
**³ÑÅÙÄ´À°¤Î¥×¥í¥°¥é¥à [#b5f9e30c]
***ÄêµÁ [#xe4a73ec]
#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#d9b4c61d]
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;
}
**¿Æµ¡¤Î¥×¥í¥°¥é¥à [#w0bdc2b7]
***ÄêµÁ [#p7c1d702]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "direction.h"
#include "drivebase.h"
#include "search.h"
#include "arm.h"
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#reaffc66]
void GotoCatchCup(){
GoForward(2);
StayWait();
Spin(-90);
StayWait();
GoForward(60);
StayWait();
Spin(90);
StayWait();
}
void GotoStackCup(){
GoForward(2);
StayWait();
Spin(-90);
StayWait();
GoBack(55);
StayWait();
Spin(90);
StayWait();
}
void BeginCupA(){
Spin(45);
StayWait();
GoForward(15);
StayWait();
}
void BeginCupB(){
Spin(-60);
StayWait();
}
void BeginCupC(){
GoForward(30);
StayWait();
}
void EndCupA(){
Spin(45 - GetDirection());
StayWait();
GoBack(10);
StayWait();
}
void EndCupB(){
Spin(-60 - GetDirection());
StayWait();
}
void EndCupC(){
Spin(-GetDirection());
StayWait();
GoBack(25);
StayWait();
}
void BeginCup(int n){
switch(n){
case 1:
DebugSound(TONE_C4);
BeginCupA();
break;
case 2:
DebugSound(TONE_C4);
BeginCupB();
break;
case 3:
DebugSound(TONE_E4);
BeginCupC();
break;
}
}
void EndCup(int n){
switch(n){
case 1:
EndCupA();
break;
case 2:
EndCupB();
break;
case 3:
EndCupC();
break;
}
}
void Cup(int phase){
int x, n;
GotoCatchCup();
BeginCup(phase);
x = PrepareCatch();
MasterCatchCup();
GoStraight(-x);
EndCup(phase);
Spin(-GetDirection());
GotoStackCup();
n = GetCupType();
BeginCup(n);
x = PrepareStack();
MasterStackCup();
GoStraight(-x);
EndCup(n);
Spin(-GetDirection());
}
***¥á¥¤¥ó¥¿¥¹¥¯ [#i8a27cb2]
task main(){
GoForward(4);
StayWait();
BTStart("slave.rxe");
InitSensor();
for(int i = 1; i <= 3; i++){
Cup(i);
}
BTStop();
}
**»Òµ¡¤Î¥×¥í¥°¥é¥à [#r0bb1a13]
***ÄêµÁ [#g2a189fc]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "arm.h"
***¥á¥¤¥ó¥¿¥¹¥¯ [#i1fb1f38]
task main(){
int msg;
while(msg != BT_FINISH){
msg = BTReceiveMessage();
switch(msg){
case BT_CATCH:
Debug(1, "Catch");
DownArm();
CatchCup();
BTSendMessage(1, false);
break;
case BT_STACK:
Debug(1, "Stack");
StackCup();
UpArm();
BTSendMessage(1, false);
break;
}
Debug(1, "Wait");
}
Debug(1, "Finish");
Wait(5000);
}
*È¿¾Ê¤È´¶ÁÛ [#dc1c78a7]
À®¸ùΨ¤ò¾å¤²¤ë¤¿¤á¤Ë²¿ÅÙ¤âÄ´À°¤·Ä¾¤·¤¿¡£
ËÜÈ֤Υí¥Ü¥³¥ó¤ÇºÇ¸å¤Î¥³¥Ã¥×¤ò¤Ä¤«¤ß»¤Í¤Æ¤·¤Þ¤Ã¤¿¡£
¤«¤Ê¤ê°ÂÄêÀ¤Î¹â¤¤µ¡ÂÎ¤È¥×¥í¥°¥é¥à¤Ë¤Ê¤Ã¤¿¡£
µ¡ÂΤγˤòºî¤Ã¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢¶¦¤ËÄ´À°¤·¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢
¥×¥í¥°¥é¥à¤òÃæ¿´¤Ë½ñ¤¤¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¤Ë´¶¼Õ¤Ç¤¢¤ë¡£
½ªÎ»¹Ô:
[[2014a/Member]]
Ìܼ¡
#contents
*²ÝÂê¤ÎÀâÌÀ [#r13fce05]
¾Ü¤·¤¯¤Ï[[2014a/Mission2]]¤ò¸«¤Æ¤¯¤À¤µ¤¤¡£
*¥á¥ó¥Ð¡¼ [#na188e61]
tadanobo
mame
dai
bgpad
*¥í¥Ü¥Ã¥ÈËÜÂÎ [#bdd03821]
&ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%);
¥¢¡¼¥à¡¢Æ¹ÂΡ¢¥É¥é¥¤¥Ö¥Ù¡¼¥¹¤È¡¢¥Ñ¡¼¥Ä¤´¤È¤Ëʬ¤±¤¿¤Î¤ÇÁȤßΩ¤Æ¡¢¼è¤ê³°¤·¤¬ÍưפǤ¢¤ë
**¥¢¡¼¥à [#y6f68c61]
&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,7%);
¢¥¢¡¼¥à¤¬²¼¤ê¤ë
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
£Â¾¤Î¥³¥Ã¥×¤Ë½Å¤Í¤ë
**¥É¥é¥¤¥Ö¥Ù¡¼¥¹ [#je9abe0b]
&ref(2014a/Member/mame/Mission2/¼Ì¿¿ 2014-08-07 16 25 32.jpg,10%);
µ¡ÂΤ¬½Å¤¤¤Î¤Ç¥Ù¡¼¥¹¤Ï´è¾æ¤Ë¤·¤¿¡£¤Þ¤¿¡¢¸åÎؤϥ¹¥à¡¼¥º¤ËÆ°¤¯¤è¤¦¤ËÂ礤ʤâ¤Î¤Ë¤·¤¿
*ºîÀï [#b8f08a37]
¥³¥Ã¥×¤ò³Î¼Â¤Ë¤È¤ë¤¿¤á¤ËÆó²ó¥³¥Ã¥×¤òȽÃǤ¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£
Ķ²»ÇÈ¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤Î°ÌÃÖ¤ò³Îǧ¤·¡¢¸÷¥»¥ó¥µ¡¼¤Ç¥³¥Ã¥×¤ÎÈÖ¹æ¤ò¸«Ê¬¤±¤ë¤è¤¦¤Ë¤·¤¿¡£
¤Þ¤¿¡¢¾¯¤·¤Ç¤â³ÑÅÙ¤¬¤º¤ì¤Ê¤¤¤è¤¦¤Ë¥³¥Ã¥×¤ò¤È¤ëËè¤Ë³ÑÅÙ¤ò½¤Àµ¤¹¤ë¥×¥í¥°¥é¥à¤òÆþ¤ì¤¿¡£
¼ç¤Ë¹ÔÆ°¤¹¤ë¤¿¤á¤Î¿Æµ¡¡¢¼ç¤Ë¥³¥Ã¥×¤ò¤È¤ë¤¿¤á¤Î»Òµ¡¤Ë¥×¥í¥°¥é¥à¤òʬ¤±¤¿¡£
*¥×¥í¥°¥é¥à [#z80241ef]
**´ðËÜ¤Î¥×¥í¥°¥é¥à [#yb10803f]
***ÄêµÁ [#ua28c278]
#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 DIST_SENSOR S2 //µ÷Υ¬ÄêÍÑĶ²»ÇÈ¥»¥ó¥µ¡¼
#define TYPE_SENSOR S3 //»æ¥³¥Ã¥×ȽÊÌÍѸ÷¥»¥ó¥µ¡¼
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#k87d7c7e]
void InitSensor(){
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
}
**µ¡ÂΤιÔÆ°¤¹¤ë¥×¥í¥°¥é¥à [#edcd34a9]
***ÄêµÁ [#w23ba2df]
#define SPEED_STRAIGHT 50
#define SPEED_SPIN 40
#define SPEED_STRAIGHT_LOW 20
#define SPEED_SPIN_LOW 30
#define STAY_WAIT 50
#define WHEEL_STRAIGHT (80 / 1715)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#h5000208]
void DebugDriveType(string type, int speed){
DebugNumber(DEBUG_DRIVETYPE, type, speed);
}
void Stay(){
Off(BOTH_WHEEL);
DebugDriveType("Stay", 0);
}
void StayWait(){
Stay();
Wait(STAY_WAIT);
}
void Drive(int speed, int rate, string type){
if(speed){
if(speed > 0){
OnFwdSync(BOTH_WHEEL, speed, rate);
}else{
OnRevSync(BOTH_WHEEL, -speed, rate);
}
DebugDriveType(type, speed);
}else{
}
}
void DriveStraight(int speed){
Drive(speed, 0, "Straight");
}
void DriveLeft(int speed){
Drive(speed, -100, "Left");
}
void DriveRight(int speed){
Drive(speed, 100, "Right");
}
void Forward(){
DriveStraight(SPEED_STRAIGHT);
}
void ForwardLow(){
DriveStraight(SPEED_STRAIGHT_LOW);
}
void Back(){
DriveStraight(-SPEED_STRAIGHT);
}
void BackLow(){
DriveStraight(-SPEED_STRAIGHT_LOW);
}
void SpinLeft(){
DriveLeft(SPEED_SPIN);
}
void SpinLeftLow(){
DriveLeft(SPEED_SPIN_LOW);
}
void SpinRight(){
DriveRight(SPEED_SPIN);
}
void SpinRightLow(){
DriveRight(SPEED_SPIN_LOW);
}
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 GoForward(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Forward();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("Forward", d);
}
Stay();
}
void GoForwardLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
ForwardLow();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("ForwardLow", d);
}
Stay();
}
void GoBack(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Back();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("Back", d);
}
Stay();
}
void GoBackLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
BackLow();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
DebugDriveType("BackLow", d);
}
Stay();
}
void GoStraight(int x){
if(x > 0){
GoForward(x);
}else{
GoBack(-x);
}
}
void GoStraightLow(int x){
if(x > 0){
GoForwardLow(x);
}else{
GoBackLow(-x);
}
}
**¥³¥Ã¥×¤òõ¤¹¤¿¤á¤Î¥×¥í¥°¥é¥à [#w225d5ad]
***ÄêµÁ [#p8f79d0c]
#define DIST_CATCH 12
#define DIST_STACK 11
#define DIST_SEARCH 6
#define SEARCH_DIRECTION1 90
#define SEARCH_DIRECTION2 120
#define DistSensor SensorUS(DIST_SENSOR)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#ie6d8cf2]
void TowardCup(int degree){
int min = DistSensor;
float dir = GetDirection();
SpinLow(-degree * 0.5);
StartDirectionTimer();
SpinLeftLow();
while(StopDirectionTimer() < degree){
if(DistSensor < min){
min = DistSensor;
dir = GetDirection();
}
}
SpinRightLow();
until(GetDirection() <= dir);
Stay();
}
int PrepareCup(int dist){
int x, y;
TowardCup(SEARCH_DIRECTION1);
x = DistSensor - DIST_SEARCH;
GoForwardLow(x);
StayWait();
TowardCup(SEARCH_DIRECTION2);
y = dist - DistSensor;
GoBackLow(y);
StayWait();
return x - y;
}
int PrepareCatch(){
return PrepareCup(DIST_CATCH);
}
int PrepareStack(){
return PrepareCup(DIST_STACK);
}
**¥³¥Ã¥×¤òȽÃǤ¹¤ë¤¿¤á¤Î¥×¥í¥°¥é¥à [#h13b91a8]
***ÄêµÁ [#uac0f5e7]
#define DIRECTION_DEBUG 1
#define DEBUG_DRIVETYPE 2
#define DEBUG_CUPTYPE 3¡¡
#define DEBUG_CUPSENSOR 3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#c68e677d]
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);
}
**Bluetooth¤Î¥×¥í¥°¥é¥à [#e5c11d2e]
***ÄêµÁ [#pea36e5f]
#define BT_CONN 1
#define BT_MAILBOX MAILBOX3
#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#m0f1915a]
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 BTStop(){
BTSendMessage(BT_FINISH, true);
}
**¥¢¡¼¥à¤Î¥×¥í¥°¥é¥à [#n48ddf38]
***ÄêµÁ [#x6c230d3]
#define DOWN_POWER 30
#define HOLD_DOWN_POWER 50
#define UP_POWER 90
#define HOLD_UP_POWER 50
#define HOLD_POWER 20
#define RELEASE_POWER 80
#define ARM1_DEGREE 90
#define ARM2_DEGREE 210
#define CUP1 55
#define CUP2 37
#define CUP3 23
#define TypeSensor SENSOR_3
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#o58a78f8]
void DownArm(){
Off(ARM_BASE);
Wait(100);
RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
OnFwd(ARM_BASE, HOLD_DOWN_POWER);
Wait(1000);
}
void UpArm(){
Off(ARM_BASE);
Wait(100);
RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE);
OnRev(ARM_BASE, HOLD_UP_POWER);
Wait(2000);
}
void HoldCup(){
OnFwd(ARM_HAND, HOLD_POWER);
while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE);
}
void ReleaseCup(){
RotateMotor(ARM_HAND, RELEASE_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;
if(TypeSensor > (CUP1 + CUP2) * 0.5){
n = 1;
}else if(TypeSensor > (CUP2 + CUP3) * 0.5){
n = 2;
}else{
n = 3;
}
DebugNumber(DEBUG_CUPTYPE, "CupNumber", n);
DebugNumber(DEBUG_CUPSENSOR, "CupSensor", TypeSensor);
return n;
}
void MasterCatchCup(){
BTSendMessage(BT_CATCH, true);
BTReceiveMessage();
}
void MasterStackCup(){
BTSendMessage(BT_STACK, true);
BTReceiveMessage();
}
**³ÑÅÙÄ´À°¤Î¥×¥í¥°¥é¥à [#b5f9e30c]
***ÄêµÁ [#xe4a73ec]
#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#d9b4c61d]
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;
}
**¿Æµ¡¤Î¥×¥í¥°¥é¥à [#w0bdc2b7]
***ÄêµÁ [#p7c1d702]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "direction.h"
#include "drivebase.h"
#include "search.h"
#include "arm.h"
***¥á¥¤¥ó¥×¥í¥°¥é¥à [#reaffc66]
void GotoCatchCup(){
GoForward(2);
StayWait();
Spin(-90);
StayWait();
GoForward(60);
StayWait();
Spin(90);
StayWait();
}
void GotoStackCup(){
GoForward(2);
StayWait();
Spin(-90);
StayWait();
GoBack(55);
StayWait();
Spin(90);
StayWait();
}
void BeginCupA(){
Spin(45);
StayWait();
GoForward(15);
StayWait();
}
void BeginCupB(){
Spin(-60);
StayWait();
}
void BeginCupC(){
GoForward(30);
StayWait();
}
void EndCupA(){
Spin(45 - GetDirection());
StayWait();
GoBack(10);
StayWait();
}
void EndCupB(){
Spin(-60 - GetDirection());
StayWait();
}
void EndCupC(){
Spin(-GetDirection());
StayWait();
GoBack(25);
StayWait();
}
void BeginCup(int n){
switch(n){
case 1:
DebugSound(TONE_C4);
BeginCupA();
break;
case 2:
DebugSound(TONE_C4);
BeginCupB();
break;
case 3:
DebugSound(TONE_E4);
BeginCupC();
break;
}
}
void EndCup(int n){
switch(n){
case 1:
EndCupA();
break;
case 2:
EndCupB();
break;
case 3:
EndCupC();
break;
}
}
void Cup(int phase){
int x, n;
GotoCatchCup();
BeginCup(phase);
x = PrepareCatch();
MasterCatchCup();
GoStraight(-x);
EndCup(phase);
Spin(-GetDirection());
GotoStackCup();
n = GetCupType();
BeginCup(n);
x = PrepareStack();
MasterStackCup();
GoStraight(-x);
EndCup(n);
Spin(-GetDirection());
}
***¥á¥¤¥ó¥¿¥¹¥¯ [#i8a27cb2]
task main(){
GoForward(4);
StayWait();
BTStart("slave.rxe");
InitSensor();
for(int i = 1; i <= 3; i++){
Cup(i);
}
BTStop();
}
**»Òµ¡¤Î¥×¥í¥°¥é¥à [#r0bb1a13]
***ÄêµÁ [#g2a189fc]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "arm.h"
***¥á¥¤¥ó¥¿¥¹¥¯ [#i1fb1f38]
task main(){
int msg;
while(msg != BT_FINISH){
msg = BTReceiveMessage();
switch(msg){
case BT_CATCH:
Debug(1, "Catch");
DownArm();
CatchCup();
BTSendMessage(1, false);
break;
case BT_STACK:
Debug(1, "Stack");
StackCup();
UpArm();
BTSendMessage(1, false);
break;
}
Debug(1, "Wait");
}
Debug(1, "Finish");
Wait(5000);
}
*È¿¾Ê¤È´¶ÁÛ [#dc1c78a7]
À®¸ùΨ¤ò¾å¤²¤ë¤¿¤á¤Ë²¿ÅÙ¤âÄ´À°¤·Ä¾¤·¤¿¡£
ËÜÈ֤Υí¥Ü¥³¥ó¤ÇºÇ¸å¤Î¥³¥Ã¥×¤ò¤Ä¤«¤ß»¤Í¤Æ¤·¤Þ¤Ã¤¿¡£
¤«¤Ê¤ê°ÂÄêÀ¤Î¹â¤¤µ¡ÂÎ¤È¥×¥í¥°¥é¥à¤Ë¤Ê¤Ã¤¿¡£
µ¡ÂΤγˤòºî¤Ã¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢¶¦¤ËÄ´À°¤·¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¡¢
¥×¥í¥°¥é¥à¤òÃæ¿´¤Ë½ñ¤¤¤Æ¤¯¤ì¤¿¥á¥ó¥Ð¡¼¤Ë´¶¼Õ¤Ç¤¢¤ë¡£
¥Ú¡¼¥¸Ì¾: