CatPot 2015-2016
CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 0:d35efbf4d62e
- Child:
- 1:e3248f278663
diff -r 000000000000 -r d35efbf4d62e main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jan 24 05:41:05 2015 +0000
@@ -0,0 +1,925 @@
+/***********************************
+ *RoboCupJunior Soccer B Open 2014
+ *Koshinestu progrum
+ *
+ *このプログラムでは以下の処理をする．
+ * LPC1114FN28/102からI2Cを用いて各種センサデータを収集
+ *
+ * データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
+ *
+ * MotorDriverにmaxonに命令
+ *
+ * ServoMotorでステアリング
+ *
+ * LCDでデバック
+ *
+ * スイッチ4つとスタートスイッチで処理を実行
+ *
+ * キッカー用のDigitalOutがどこかに入る
+ *
+ *************************
+ *
+ *Pin Map
+ *
+ * p5,p6,p7,p29,p30 >> SPI >>Stepping
+ *
+ * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
+ *
+ * p13,p14 >> Serial >> Motor
+ *
+ * p22~p26 >> DebugSw and StartSw
+ *
+ * p27,p28 >> I2C >> DebugLCD
+ *
+ * p21 >> ServoMotor
+ *
+ ******************************/
+
+
+/*
+
+****以下IRはNighは距離が近いものとする．
+
+*/
+
+
+
+
+
+
+#include "mbed.h"
+#include "L6470.h"
+#include "PID.h"
+#include "AQM0802A.h"
+#include "HMC6352.h"
+#include "Servo.h"
+
+#include <math.h>
+#include <sstream>
+
+/*回り込みの計算用*/
+#define PI 3.141593/*割と早めにきってある*/
+#define SHORT_LEN 15 /*cm換算 楕円のB辺の長さを定義しておく*/
+
+#define ADDRESS_R 0xA0
+#define ADDRESS_L 0xC0
+
+/*BusIn sw 入力値*/
+#define Calibration 0x01
+#define Kicker 0x02
+#define Debug1 0x04
+#define Debug2 0x08
+#define StartS 0x10
+
+#define READ_IR 0x01 //送る物指定
+#define READ_PING 0x02
+
+/*Servo*/
+#define HOME 0.0
+#define UNIT 360.0 / 12
+#define H1 HOME + 1*UNIT
+#define H2 HOME + 2*UNIT
+#define H3 HOME + 3*UNIT
+#define H4 HOME - 2*UNIT
+#define H5 HOME - 1*UNIT
+#define H6 HOME
+#define H7 HOME + 1*UNIT
+#define H8 HOME + 2*UNIT
+#define H9 HOME - 3*UNIT
+#define H10 HOME - 2*UNIT
+#define H11 HOME - 1*UNIT
+#define H12 HOME
+
+#define F90 +90.0
+#define L90 -90.0
+
+//ex.) Servo::position(float degrees)
+//○ Servo.position(HOME);
+//× Servo=HOME;
+
+BusIn Sw(p22,p23,p24,p25,p26);
+DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
+
+I2C Sensor(p9,p10);//sda,scl
+HMC6352 hmc6352(p9,p10);
+AQM0802A Lcd(p28,p27);//sda,scl
+L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
+
+Servo Servo(NC);
+
+Serial Motor(p13,p14);//tx,rx
+Serial pc(USBTX,USBRX);
+
+DigitalOut Kick(p21);
+
+extern string StringFIN;
+extern void array(int,int,int,int);
+
+int speed[4] = {0};
+
+typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
+
+/*SensorData回収↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/
+unsigned int IrReceiveR(){
+ /*順位解析版
+ Slave側はIRを要求した場合IRのみを返してくるとする．
+ irは値が大きいほうが近いと仮定する
+
+
+ */
+ char data_r[3],data_l[3];
+ bool val;
+ Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+
+ val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
+ Led[0] = val;
+ wait_ms(5);
+ val = Sensor.read(ADDRESS_L|1, data_l, 3);
+ Led[0] = !val;
+
+ if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+ if((data_r[0] == 0)&&(data_l[0] == 0)){
+ return 12;
+ }
+ if(data_r[0] == 0){
+ return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
+ }
+ return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
+
+ }
+
+ if(data_r[2]>data_l[2]){
+ if(data_r[2]>data_l[1]){
+ return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
+ }
+
+ return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
+
+ }else{
+ if(data_l[2]>data_r[1]){
+ return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
+ }
+
+ return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);
+ }
+
+}
+
+uint8_t IrReceiveS(){
+ /*単純解析版 一番大きい位置だけ返す
+
+ Slave側はIRを要求した場合IRのみを返してくるとする．
+ irは値が大きいほうが近いと仮定する
+
+ */
+ char data_r[3],data_l[3];
+ bool val;
+ Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+
+ val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
+ Led[0] = val;
+ wait_ms(5);
+ val = Sensor.read(ADDRESS_L|1, data_l, 3);
+ Led[0] = !val;
+
+ if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+ if((data_r[0] == 0)&&(data_l[0] == 0)){
+ return 12;
+ }
+ if(data_r[0] == 0){
+ return data_l[0]/6+6;
+ }
+ return data_r[0]/6;
+ }
+
+ if(data_r[2]>data_l[2]){
+ return data_r[0]/6;
+ }
+ return data_l[0]/6+6;
+
+}
+uint8_t IrReceiveM(){
+ /*値解析版 一番近い場所の値を返す（位置はとりあえずない）
+ Slave側はIRを要求した場合IRのみを返してくるとする．
+ irは値が大きいほうが近いと仮定する
+
+
+ */
+ char data_r[3],data_l[3];
+ bool val;
+ Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+
+ val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
+ Led[0] = val;
+ wait_ms(5);
+ val = Sensor.read(ADDRESS_L|1, data_l, 3);
+ Led[0] = !val;
+
+ if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+ if((data_r[0] == 0)&&(data_l[0] == 0)){
+ return 255;
+ }
+ if(data_r[0] == 0){
+ return data_l[1];
+ }
+ return data_r[1];
+ }
+
+
+ if(data_r[2]>data_l[2]){
+ return data_r[1];
+
+ }
+
+ return data_l[1];
+
+}
+unsigned int IrReceiveSM(){//16bit()
+ /*値解析版 一番大きい値とその位置を返す
+ Slave側はIRを要求した場合IRのみを返してくるとする．
+ irは値が大きいほうが近いと仮定する
+
+
+ */
+ char data_r[3],data_l[3];
+ bool val;
+ Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+
+ val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
+ Led[0] = val;
+ wait_ms(5);
+ val = Sensor.read(ADDRESS_L|1, data_l, 3);
+ Led[0] = !val;
+
+ if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+ if((data_r[0] == 0)&&(data_l[0] == 0)){
+ return 12<<8+255;
+ }
+ if(data_r[0] == 0){
+ return (data_l[0]/6+6)<<8 + data_l[1];
+ }
+ return data_r[0]/6<<8 + data_r[1];
+ }
+
+
+ if(data_r[2]>data_l[2]){
+ return data_r[0]/6<<8 + data_r[1];
+
+ }
+ return ((data_l[0]/6)+6) <<8 + data_l[1];
+
+
+}
+uint8_t LineReceive(){
+ //先に要求しない場合はLineの状況を送信すること．//4bit //前ー右ー後ろー左
+ char data_r[2],data_l[2];
+ bool val;
+ //example val = slave.read(address,data,length,repeat);
+ val = Sensor.read(ADDRESS_R|1, data_r, 1);
+ Led[1] = val;
+ wait_ms(5);
+ val = Sensor.read(ADDRESS_L|1, data_l, 1);
+ Led[1] = !val;
+
+
+ return data_r[0]<<2 + data_l[0];
+
+
+}
+
+
+void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left
+ char ReadSelect[1] = {READ_PING};
+ bool val;
+ val = Sensor.write(ADDRESS_R|0, ReadSelect , 1);
+ Led[2] = val;
+ val = Sensor.read(ADDRESS_R|1, ping, 2);
+ Led[2] = !val;
+
+
+}
+
+void PingReceiveFB(char ping[]){//ping[0]==FRONT,ping[1]==BACK
+ char ReadSelect[1] = {READ_PING};
+ bool val;
+ val = Sensor.write(ADDRESS_L|0, ReadSelect , 1);
+ Led[2] = val;
+ val = Sensor.read(ADDRESS_L|1, ping, 2);
+ Led[2] = !val;
+
+
+}
+
+/*SensorData回収↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/
+
+/*モーター駆動処理↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/
+
+void rotate(unsigned int times, bool dir){
+ /*
+ *回転速度，回転方向，回転角度等設定変更ののち回転．
+ * this is 使わなそうである、
+ */
+
+
+
+
+
+}
+
+void move(int vr, int vl){
+ double pwm[4] = {0};
+ uint8_t i = 0;
+ pwm[0] = vr/10.0;
+ pwm[1] = vl/10.0;
+ pwm[2] = 0;
+ pwm[3] = 0;
+
+ for(i = 0; i < 4; i++){
+ if(pwm[i] > 100){
+ pwm[i] = 100;
+ }else if(pwm[i] < -100){
+ pwm[i] = -100;
+ }
+ speed[i] = pwm[i];
+ }
+}
+
+void ControlRobo0(int *CompassDef)//LeftFront 11時
+{}
+void ControlRobo1(int *CompassDef)//Front 12時
+{
+/*前にボールがある場合の動作*/
+ int Compass;
+ char Ping[2];
+ uint8_t Line,LineIr;
+ unsigned int IrNumMax;
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ if(!((Compass>=150)&&(Compass<=210))){
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ return;
+ }
+ if(IrReceiveM()<=150){//適当な150
+ /*ステッピングを正面に設定*/
+ /*busy_wait()*/
+ /*モーターを前進*/
+ return;
+ }
+ PingReceiveRL(Ping);
+ if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
+ /*ステッピングを少しだけ傾ける*/
+ /*モーターを左右差つけて回す(ロボットは少し傾く)*/
+ /*busy_wait()*/
+ /*モーターを前進*/
+ while(1){
+ Line = LineReceive();
+
+ if(Line){ //ラインを検知していないか
+ LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
+ while(LineIr){
+ move(0,0);//
+ Led[1] = Led[2] = Led[3] = 1;
+
+ LineIr = LineReceive() & (IrReceiveS()/3 + 1);
+ }
+ Led[1] = Led[2] = Led[3] = 0;
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ return;
+ }
+ Kick = 1;
+ wait_ms(200);
+ Kick = 0;
+
+ IrNumMax = IrReceiveSM();//位置と大きさ
+
+ if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す.
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ return;
+ }
+
+ if((IrNumMax&0x00FF)<150){
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ return;
+ }
+
+ }
+
+ }else if((Ping[0]>40)&&(Ping[1]>90)){
+
+ }else{
+
+ }
+
+}
+void ControlRobo2(int *CompassDef)//RightFront 1時
+{
+
+ int Compass;
+ char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+ char ir_num,line;//わかりやすい名前
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ if(!((Compass>=150)&&(Compass<=210))){
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ move(0,0);
+ return;
+ }
+ Step.GoHome();
+ /*irデータ取得 1,2位の場所、一位の距離*/
+ if(!(Ir[0]%13 ==2)){
+ return;
+ }
+ Step.BusyWait(0);
+
+ if(Ir[1]>=100){//近い場合
+ //2位を含んだ計算
+ /*motorset*/
+ /*stepset*/
+ return;
+ }
+
+ /*2位を含んだ計算*/
+ /*計算された分stepmove*/
+ Step.BusyWait(0);
+ move(20,20);
+
+
+ /*line,ir get*/
+ do{
+ if(line){
+ move(0,0);
+ return;
+ }
+
+ /*line,ir get*/
+ }while(ir_num == 2);
+
+ move(0,0);
+
+
+
+}
+void ControlRobo3(int *CompassDef)//RightFront 2時
+{
+ int Compass;
+ char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+ char ir_num,line;//わかりやすい名前
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ if(!((Compass>=150)&&(Compass<=210))){
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ move(0,0);
+ return;
+ }
+ Step.GoHome();
+ /*irデータ取得 1,2位の場所、一位の距離*/
+ if(!(Ir[0]%13 ==3)){
+ return;
+ }
+ Step.BusyWait(0);
+
+ if(Ir[1] >= 100){
+ /*2位を含めた計算*/
+ /*move()*/
+ /*Stepset*/
+ return;
+ }
+ /*2位を含めた計算*/
+ /*Stepsetting 距離等により角度が変わる*/
+ Step.BusyWait(0);
+ move(-20,-20);
+ /*ir,line check*/
+ do{
+ if(line){
+ move(0,0);
+ return;
+ }
+ /*line,ir check*/
+ }while(ir_num == 3);
+
+ move(0,0);
+
+
+}
+void ControlRobo4(int *CompassDef)//Right 3時
+{
+ int a = 50;
+ unsigned int ir_sm;
+ int kakudo = -90;
+ double value = 0;
+ double v = 50;/* cm/s */
+ double x = 20;/*モーターを回す割合,今は適当*/
+
+ ir_sm = IrReceiveSM();
+ if(ir_sm&0xF00>>8 == 4){
+ return;
+ }
+ a = 300 - ir_sm&0x00FF ;//適当な計算//300は勘
+ value = 10 * a * SHORT_LEN + 3*(a * a + SHORT_LEN * SHORT_LEN);
+ value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v;
+
+ if(300 - ir_sm&0x00FF >200){
+ Step.Step(-10);//適当
+ Step.BusyWait(0);
+ }
+
+ /*SetPerm()
+ *valueに応じた何かを計算する
+ *ACCまたはMAX_SPEEDをいじればいい気がする
+ *
+ */
+ Step.Step(kakudo+10);
+
+ /*x = v*何か*/
+ move(-x,-x);
+
+
+}
+void ControlRobo5(int *CompassDef)//RightBack 4時
+{
+ int Compass;
+ char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+ char ir_num,line;//わかりやすい名前
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ if(!((Compass>=150)&&(Compass<=210))){
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ move(0,0);
+ return;
+ }
+ Servo.position(HOME);
+
+ /*irデータ取得 1,2位の場所、一位の距離*/
+ if(!(Ir[0]%13 == 5)){
+ return;
+ }
+
+
+ if(Ir[1] >=100){//近い
+ /*計算*/
+
+ Servo.position(H1);
+ move(-20,-20);
+ /*Step.setpaaa
+ Step.move();-180+初期位置
+ */
+ /*ir,linecheck*/
+ do{
+ if(line){
+ move(0,0);
+ return;
+ }
+ if(!(ir_num >1 && ir_num <=5)){
+ move(0,0);
+ return;
+ }
+ /*ir,line check*/
+ }while(1);
+ }
+ //遠い
+
+ /*計算*/
+
+ Servo.position(H4);
+
+ move(-20,-20);
+
+ /*ir,linecheck*/
+ do{
+ if(line){
+ move(0,0);
+ return;
+ }
+ if(!(ir_num >1 && ir_num <=5)){
+ move(0,0);
+ }
+ /*ir,line check*/
+ }while(1);
+
+
+}
+void ControlRobo6(int *CompassDef)//BackRight 5時
+{
+
+
+}
+void ControlRobo7(int *CompassDef)//Back 6時
+{
+ /*****
+ * this function is drawing a semicircle.
+ * semicircle manipulated ir_data&ping_data.
+ *
+ * 変数名は後から変えるためにわかりやすい名前にしておく
+ **/
+ uint8_t ir_num;
+ uint8_t pingl,pingr;
+
+ /*
+ ir(1位,2位,必要なら距離も)と超音波のデータを取得
+ */
+ if(ir_num==7){//一致しているかどうか念のため
+ return ;
+ }
+ if(pingr<pingl){//本当ならば壁と垂直かどうか、確認すべき。コンパスや超音波の合計を見れば可能
+ /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
+ /*半円なのであらかじめステッピングを回転させる必要がある*/
+ /*モーター設定*/
+ /*ステッピング設定*/
+ return;
+ }
+ /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
+ /*半円なのであらかじめステッピングを回転させる必要がある*/
+ /*モーター設定*/
+ /*ステッピング設定*/
+
+
+ }
+void ControlRobo8(int *CompassDef)//BackLeft 7時
+{}
+void ControlRobo9(int *CompassDef)//LeftBack 8時
+{}
+void ControlRobo10(int *CompassDef)//Left 9時
+{}
+void ControlRobo11(int *CompassDef)//LeftFront 10時
+{}
+void GoHome(int *CompassDef)//Ball is not found.
+{
+ /*line検知をしないバージョン*/
+ int Compass;
+ char ping[2] = {0};
+
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ if(!((Compass>=150)&&(Compass<=210))){
+ while(!((Compass>=150)&&(Compass<=210))){
+ move(20,-20);//適当な回転。
+ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+ }
+ /*return;してもいいかもしれない*/
+ }
+ Step.GoHome();
+ Step.BusyWait(0);
+
+ PingReceiveFB(ping);
+ if(ping[1] >=60){//後ろからの距離60cm
+ while(ping[1] >=60){
+ move(-20,-20);
+ PingReceiveFB(ping);
+ }
+ }
+
+ move(0,0);//stop
+
+}
+/*モーター駆動処理↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/
+
+uint8_t SwRead(){
+/******
+ *返却値はスイッチの状況
+ *参照元::aaaaa
+ *
+ *Calibration = 0x01;
+ *Kicker = 0x02;
+ *Debug1 = 0x04;
+ *Debug2 = 0x08;
+ *StartS = 0x10;
+ *
+ *****/
+ uint8_t i,temp,temp2;
+ temp = Sw;
+ if(!(temp == Calibration
+ ||temp == Kicker
+ ||temp == Debug1
+ ||temp == Debug2
+ ||temp == StartS)) return 0;/*スイッチは押されていない状況*/
+ if(temp !=0x00){
+ for(i = 0; i < 50; i++);
+ temp2 = Sw;
+ if(temp == temp2){
+ return temp;
+ }
+ }
+ return 0;
+}
+
+//通信（モータ用）
+void tx_motor(){
+ array(speed[0],speed[1],speed[3],speed[2]);
+ Motor.printf("%s",StringFIN.c_str());
+}
+
+void SetUp(int *compass_def){
+ /*初期化*/
+ Kick = 0;
+ Sw.mode(PullUp);
+ Motor.baud(115200); //ボーレート設定
+ Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
+ Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
+ move(0,0);//停止
+
+ //Step.Resets();
+ //Step.ReleseSW(0,1);
+
+ Servo.calibrate(0.0005, 120.0);
+ Servo.position(HOME);//タイヤを前に向ける
+
+
+ hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ *compass_def = (hmc6352.sample() / 10);
+
+ Lcd.cls();
+
+ /*初期化終了*/
+
+
+}
+void StartLoop(){
+ /*
+ *スイッチが押されるまでロボットはスタートしない．
+ *
+ *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
+ *
+ *switch割り当て
+ *1.コンパスのキャリブレーション実行スイッチ
+ *2.キッカーのキック(check用)
+ *3,4.自由
+ *5.StartSw
+ */
+ uint8_t State = 0;
+
+ while(1){
+
+ State = SwRead();
+ if(State == 0) continue;
+
+ if(State == Calibration){
+ /*calibration command enter...
+ *
+ *
+ */
+ continue;
+ }
+ if(State == Kicker){
+ /*
+ *kicker check
+ *
+ *
+ */
+ continue;
+ }
+ if(State == Debug1){
+ /* debug command free
+ *
+ *display out to selected 3 menus. compass, ir, ping, line,etc...
+ *
+ **/
+
+ }
+ if(State == Debug2){
+ /* debug command free
+ *
+ * decide movement of the beginning.
+ *
+ *
+ **/
+
+ }
+ if(State == StartS){
+ /*game start...*/
+ return;
+ }
+
+ }
+
+}
+int main() {
+
+ /*Ir*/
+ uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
+
+ /*Line*/
+ uint8_t Line;
+
+ /*Compass*/
+ int CompassDef = 0, Compass = 0;
+
+ /*State */
+ //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
+ uint8_t LineIr = NA;
+
+
+
+ /*関数ポインタ*/
+ void (*ControlRobo[13])(int *);//irの位置による分岐
+ ControlRobo[0] = ControlRobo0;
+ ControlRobo[1] = ControlRobo1;
+ ControlRobo[2] = ControlRobo2;
+ ControlRobo[3] = ControlRobo3;
+ ControlRobo[4] = ControlRobo4;
+ ControlRobo[5] = ControlRobo5;
+ ControlRobo[6] = ControlRobo6;
+ ControlRobo[7] = ControlRobo7;
+ ControlRobo[8] = ControlRobo8;
+ ControlRobo[9] = ControlRobo9;
+ ControlRobo[10] = ControlRobo10;
+ ControlRobo[11] = ControlRobo11;
+ ControlRobo[12] = GoHome;
+
+
+ SetUp(&CompassDef);/*set up routine*/
+
+ StartLoop(); /*loop stat, switch push end*/
+
+ /*
+ 前を向くように設定をするべき
+ アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
+ */
+ while(1){//前を向かせるwhile
+ Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
+ if(Compass == 3) break;//前を向いたら抜ける。
+ }
+ while(0) {
+
+ Line = LineReceive();
+
+ if(Line){ //ラインを検知していないか
+ LineIr = Line & (IrReceiveS()/3 + 1);//計(lineとirの方位を合わせる。
+ while(LineIr){
+ move(0,0);//
+ Led[1] = Led[2] = Led[3] = 1;
+
+ LineIr = LineReceive() & (IrReceiveS()/3 + 1);
+ }
+ Led[1] = Led[2] = Led[3] = 0;
+ continue;
+
+ }
+
+ if(Step.BusyCheck()){//ステッピングが動いていないか
+ continue;
+
+ }
+
+ IrNum = IrReceiveS();
+
+ (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。
+
+ //wait_ms(0);
+ }
+
+ while(1){
+ /*デモプログラム
+ *
+ *ぐるぐる
+ *
+ *Servoの場合、ボールを中心にしなければ円軌道を作るのは困難
+ */
+ move(30,30);
+
+ wait(1);
+
+ move(-30,-30);
+ move(0,0);
+
+ Step.Run(1,1200);
+ move(10,10);
+ wait(1.5);
+
+ move(0,0);
+ Step.SoftStop();
+
+ wait(0.5);
+
+ Step.GoHome();
+
+ wait(1.5);
+
+
+
+ }
+
+}