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(...
?コップをつかむ
&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 24 50....
?アームが下りる
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
?他のコップに重ねる
**ドライブベース [#je9abe0b]
&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 25 32....
機体が重いのでベースは頑丈にした。また、後輪はスムーズに...
*作戦 [#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) *...
DebugDriveType("Forward", d);
}
Stay();
}
void GoForwardLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
ForwardLow();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) *...
DebugDriveType("ForwardLow", d);
}
Stay();
}
void GoBack(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Back();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) *...
DebugDriveType("Back", d);
}
Stay();
}
void GoBackLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
BackLow();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) *...
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 + rightMoterC...
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(...
?コップをつかむ
&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 24 50....
?アームが下りる
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
?他のコップに重ねる
**ドライブベース [#je9abe0b]
&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 25 32....
機体が重いのでベースは頑丈にした。また、後輪はスムーズに...
*作戦 [#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) *...
DebugDriveType("Forward", d);
}
Stay();
}
void GoForwardLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
ForwardLow();
until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) *...
DebugDriveType("ForwardLow", d);
}
Stay();
}
void GoBack(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
Back();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) *...
DebugDriveType("Back", d);
}
Stay();
}
void GoBackLow(int x){
int d;
float p = MotorRotationCount(LEFT_WHEEL);
BackLow();
until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) *...
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 + rightMoterC...
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]
成功率を上げるために何度も調整し直した。
本番のロボコンで最後のコップをつかみ損ねてしまった。
かなり安定性の高い機体とプログラムになった。
機体の核を作ってくれたメンバー、共に調整してくれたメンバ...
プログラムを中心に書いてくれたメンバーに感謝である。
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
