2014a/Member/tadanobo/Mission2
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2014a/Member]]
目次
#contents
*メンバー [#ff9aa291]
・tadanobo（私めでございます。只之某（ただのなにがし）で...
・mame
・dai
・bgpat
*課題 [#o86037ce]
&ref(2014a/Member/tadanobo/Mission2/課題２.png,80%);
・上図のA B C 1 2 3 は紙コップを示し、上図の左半分の３つ...
・上図右上のstartから始めて、左半分エリアからカップを回収...
詳しい説明は[[2014a/Mission2]]を見てください。
*全体の流れ [#ed2287b1]
&ref(2014a/Member/tadanobo/Mission2/過程２’.png,80%);
・右上のstartから右に90°回転して左上の点線三角の位置に移...
?１個目の時、左に135°回転してカップを回収する
?２個目の時、左に45° 〃
?３個目の時、左に90°回転しカップ手前まで前進してから...
・カップを回収したら点線三角の位置に戻ってからstartに移動...
**補足 [#c07d3db4]
・カップはマシンの正面についている超音波センサー（縦にな...
確実性を高めるため、カップに接近してからマシンを左右に旋...
また、この動作を２回することでさらに正確にカップに正対で...
・始めはライントレース（[[2014a/Member/tadanobo/Mission1]...
しかし最終的には、上記のようにstart位置、となりのエリアの...
*ロボット [#n9c6e95b]
**本体 [#hd0f3b79]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-07-25 16 36...
マシンはドライブベースの上に本体を横に並べて設置していま...
正面から見て右が親機（master）、左が子機（slave）です。親...
**ドライブベース [#h4fed50b]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 25...
後輪は始めキャスターのようにクルクル回るものでしたが、接...
このホイールにしたところ本体の重量を支えていても安定し、...
**アーム [#d41edc52]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 24...
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 24...
アームは上から下ろしてカップをつかむようになっています。
&ref(2014a/Member/tadanobo/Mission2/つかむ１.png,50%);
&ref(2014a/Member/tadanobo/Mission2/つかむ２.png,50%);
&ref(2014a/Member/tadanobo/Mission2/つかむ４.png,50%);
カップをつかんでアームを上げた際、一度つめを上げてカップ...
下ろすことでカップを安定してつかむと同時にアームについて...
*プログラム [#ea7172f8]
”()”はそのタイトル以下に書かれているプログラムの名前を示...
**ドライブベース [#bc9f6bba]
***各モーター・センサーの定義（命名） ”robot.h” [#e331...
#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 //紙コップ判別用光センサー
void InitSensor(){
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
}
***各動作の定義 [#kaff63fa] ”drivebase.h” [#p39e563b]
#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)
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);
}
}
**車体の向き（自身がフィールド上においてどの向きに向いて...
#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
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 + rightMoter...
leftMoterCount = out;
rightMoterCount = in;
DebugNumber(DIRECTION_DEBUG, "Direction", direction);
return direction;
}
void StartDirectionTimer(){
directionTimer = GetDirection();
}
float StopDirectionTimer(){
return GetDirection() - directionTimer;
}
これはタイヤの半径・車体の旋回時における回転半径などを用...
具体的には下図の赤い四角内の式で車体の角度が得られます。...
&ref(2014a/Member/tadanobo/Mission2/角度計算説明.png,50%);
&ref(2014a/Member/tadanobo/Mission2/説明２.png,50%);
※上図のαｙとαｘに関して・・・半径αの円が角度ｚだけ回った...
※旋回の時は結果的に右上図にような式になります。厳密には左...
このように自身の向きを計算させるとマシンの操作が容易にな...
最初startにまっすぐ置いたとして、それを0°とします。（因み...
たとえばここから車体を90°右に向けたい時、マシンに「自身の...
また、向きを最初の状態にリセットしたい時は「自身の向きが0...
**カップを探す ”search.h”[#u91ae062]
#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)
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);
}
まず最初に首を左右に振ってカップのある方向を計ってその方...
**アームのプログラム ”arm.h” [#id3010c4]
***定義文 [#z664db1d]
#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
***サブルーチン [#b20e0b81]
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();
}
**Bluetooth（２つの本体間での通信） ”bluetooth.h” [#u826...
#define BT_CONN 1
#define BT_MAILBOX MAILBOX3
#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 3
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);
}
この通信では主に
?親機が子機にプログラムを起動させる（親機による子機内のプ...
?２つの本体間でメッセージのやりとり
の２つをします。
たとえばマシンがカップをつかむときは、
?親機が子機にカップをつかむプログラムを起動させる。親機は...
?子機がアームを動かしてカップを掴む
?カップをつかみ終えたら、子機は親機に既定のメッセージを送る
?親機は子機から既定のメッセージを受け取ると、それを合図と...
ということをしています。親機はプログラムの遠隔起動により...
**メインタスク?―slave― （子機のプログラム） [#r750a95f]
***定義文 [#jc0466b3]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "arm.h"
このincludeとは、""内のプログラムを含む、という意味です。...
このようにincludeを使うことでプログラムを短くまとめること...
***メインタスク [#r5318f45]
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);
}
**メインタスク?―master― （親機のプログラム） [#vc45c1a4]
***定義文 [#bd09759f]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "direction.h"
#include "drivebase.h"
#include "search.h"
#include "arm.h"
***サブルーチン [#gf1ff817]
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());
}
***メインタスク [#v4dde9c9]
task main(){
GoForward(4);
StayWait();
BTStart("slave.rxe");
InitSensor();
for(int i = 1; i <= 3; i++){
Cup(i);
}
BTStop();
}
*感想 [#v76af6e2]
課題１に比べ、チーム人数は４人に増えたことに加え準備の時...
マシンは私の制作した試作機を皆で改良して完成させ、プログ...
プログラムに関して、大部分を手がけた上、車体に自身の向き...
**反省点 [#n2ed8d70]
・この度ロボコンでは２個のカップを重ねるに至ったものの、...
・車体には自身の向きを計算させていたものの、移動している...
**良かった点 [#t61d5a03]
・本体とアーム部分はBluetoohによる通信で連携させたが、ス...
・超音波センサーで探しながらのカップへの接近がうまくいっ...
終了行:
[[2014a/Member]]
目次
#contents
*メンバー [#ff9aa291]
・tadanobo（私めでございます。只之某（ただのなにがし）で...
・mame
・dai
・bgpat
*課題 [#o86037ce]
&ref(2014a/Member/tadanobo/Mission2/課題２.png,80%);
・上図のA B C 1 2 3 は紙コップを示し、上図の左半分の３つ...
・上図右上のstartから始めて、左半分エリアからカップを回収...
詳しい説明は[[2014a/Mission2]]を見てください。
*全体の流れ [#ed2287b1]
&ref(2014a/Member/tadanobo/Mission2/過程２’.png,80%);
・右上のstartから右に90°回転して左上の点線三角の位置に移...
?１個目の時、左に135°回転してカップを回収する
?２個目の時、左に45° 〃
?３個目の時、左に90°回転しカップ手前まで前進してから...
・カップを回収したら点線三角の位置に戻ってからstartに移動...
**補足 [#c07d3db4]
・カップはマシンの正面についている超音波センサー（縦にな...
確実性を高めるため、カップに接近してからマシンを左右に旋...
また、この動作を２回することでさらに正確にカップに正対で...
・始めはライントレース（[[2014a/Member/tadanobo/Mission1]...
しかし最終的には、上記のようにstart位置、となりのエリアの...
*ロボット [#n9c6e95b]
**本体 [#hd0f3b79]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-07-25 16 36...
マシンはドライブベースの上に本体を横に並べて設置していま...
正面から見て右が親機（master）、左が子機（slave）です。親...
**ドライブベース [#h4fed50b]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 25...
後輪は始めキャスターのようにクルクル回るものでしたが、接...
このホイールにしたところ本体の重量を支えていても安定し、...
**アーム [#d41edc52]
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 24...
&ref(2014a/Member/tadanobo/Mission2/写真 2014-08-07 16 24...
アームは上から下ろしてカップをつかむようになっています。
&ref(2014a/Member/tadanobo/Mission2/つかむ１.png,50%);
&ref(2014a/Member/tadanobo/Mission2/つかむ２.png,50%);
&ref(2014a/Member/tadanobo/Mission2/つかむ４.png,50%);
カップをつかんでアームを上げた際、一度つめを上げてカップ...
下ろすことでカップを安定してつかむと同時にアームについて...
*プログラム [#ea7172f8]
”()”はそのタイトル以下に書かれているプログラムの名前を示...
**ドライブベース [#bc9f6bba]
***各モーター・センサーの定義（命名） ”robot.h” [#e331...
#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 //紙コップ判別用光センサー
void InitSensor(){
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
}
***各動作の定義 [#kaff63fa] ”drivebase.h” [#p39e563b]
#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)
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);
}
}
**車体の向き（自身がフィールド上においてどの向きに向いて...
#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
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 + rightMoter...
leftMoterCount = out;
rightMoterCount = in;
DebugNumber(DIRECTION_DEBUG, "Direction", direction);
return direction;
}
void StartDirectionTimer(){
directionTimer = GetDirection();
}
float StopDirectionTimer(){
return GetDirection() - directionTimer;
}
これはタイヤの半径・車体の旋回時における回転半径などを用...
具体的には下図の赤い四角内の式で車体の角度が得られます。...
&ref(2014a/Member/tadanobo/Mission2/角度計算説明.png,50%);
&ref(2014a/Member/tadanobo/Mission2/説明２.png,50%);
※上図のαｙとαｘに関して・・・半径αの円が角度ｚだけ回った...
※旋回の時は結果的に右上図にような式になります。厳密には左...
このように自身の向きを計算させるとマシンの操作が容易にな...
最初startにまっすぐ置いたとして、それを0°とします。（因み...
たとえばここから車体を90°右に向けたい時、マシンに「自身の...
また、向きを最初の状態にリセットしたい時は「自身の向きが0...
**カップを探す ”search.h”[#u91ae062]
#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)
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);
}
まず最初に首を左右に振ってカップのある方向を計ってその方...
**アームのプログラム ”arm.h” [#id3010c4]
***定義文 [#z664db1d]
#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
***サブルーチン [#b20e0b81]
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();
}
**Bluetooth（２つの本体間での通信） ”bluetooth.h” [#u826...
#define BT_CONN 1
#define BT_MAILBOX MAILBOX3
#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 3
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);
}
この通信では主に
?親機が子機にプログラムを起動させる（親機による子機内のプ...
?２つの本体間でメッセージのやりとり
の２つをします。
たとえばマシンがカップをつかむときは、
?親機が子機にカップをつかむプログラムを起動させる。親機は...
?子機がアームを動かしてカップを掴む
?カップをつかみ終えたら、子機は親機に既定のメッセージを送る
?親機は子機から既定のメッセージを受け取ると、それを合図と...
ということをしています。親機はプログラムの遠隔起動により...
**メインタスク?―slave― （子機のプログラム） [#r750a95f]
***定義文 [#jc0466b3]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "arm.h"
このincludeとは、""内のプログラムを含む、という意味です。...
このようにincludeを使うことでプログラムを短くまとめること...
***メインタスク [#r5318f45]
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);
}
**メインタスク?―master― （親機のプログラム） [#vc45c1a4]
***定義文 [#bd09759f]
#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "direction.h"
#include "drivebase.h"
#include "search.h"
#include "arm.h"
***サブルーチン [#gf1ff817]
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());
}
***メインタスク [#v4dde9c9]
task main(){
GoForward(4);
StayWait();
BTStart("slave.rxe");
InitSensor();
for(int i = 1; i <= 3; i++){
Cup(i);
}
BTStop();
}
*感想 [#v76af6e2]
課題１に比べ、チーム人数は４人に増えたことに加え準備の時...
マシンは私の制作した試作機を皆で改良して完成させ、プログ...
プログラムに関して、大部分を手がけた上、車体に自身の向き...
**反省点 [#n2ed8d70]
・この度ロボコンでは２個のカップを重ねるに至ったものの、...
・車体には自身の向きを計算させていたものの、移動している...
**良かった点 [#t61d5a03]
・本体とアーム部分はBluetoohによる通信で連携させたが、ス...
・超音波センサーで探しながらのカップへの接近がうまくいっ...
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
