Diff: main.cpp

Revision:

4:8444360f08e2

Parent:

3:8d718ddb84c9

Child:

5:3d68334aab20

--- a/main.cpp	Sat Dec 27 07:33:07 2014 +0000
+++ b/main.cpp	Mon Dec 29 07:24:37 2014 +0000
@@ -55,15 +55,26 @@
 #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
 
 
-DigitalIn   DebugSw[4] = {p22,p23,p24,p25};
-DigitalIn   StartSw(p26,PullUp);
+
+BusIn   Sw(p22,p23,p24,p25,p26);
 DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};
 
 I2C         Sensor(p9,p10);//sda,scl
@@ -83,7 +94,7 @@
 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
 
 
-unsigned int IrReceiveR(uint8_t *far){
+unsigned int IrReceiveR(){
     /*順位解析版
     Slave側はIRを要求した場合IRのみを返してくるとする．
     irは値が大きいほうが近いと仮定する
@@ -349,8 +360,36 @@
 {}
 void ControlRobo4(int *CompassDef)//Right
 {
+    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
@@ -371,7 +410,34 @@
 {}
 
 
-
+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(){
@@ -383,10 +449,7 @@
     /*初期化*/
     int Compass;
     Kick = 0;
-    DebugSw[0].mode(PullUp);
-    DebugSw[1].mode(PullUp);
-    DebugSw[2].mode(PullUp);
-    DebugSw[3].mode(PullUp);
+    Sw.mode(PullUp);
     Motor.baud(115200);                             //ボーレート設定
     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
@@ -397,9 +460,14 @@
 
     hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     *compass_def = (hmc6352.sample() / 10);
-    
-    Compass = ((hmc6352.sample() / 10) + 540 - *compass_def) % 360;
-    /*前を向くように設定をするべき*/
+    /*
+    前を向くように設定をするべき→最初っから前向いてるし
+    アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
+    */
+    while(1){
+        Compass = ((hmc6352.sample() / 10) + 540 - *compass_def) % 360;
+        if(Cmpass == 3) break;//前を向いたら抜ける。
+    }
     Lcd.cls();
     
     /*初期化終了*/
@@ -412,8 +480,48 @@
      *
      *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
      *
-     *
+     *switch割り当て
+     *1.コンパスのキャリブレーション実行スイッチ
+     *2.キッカーのキック
+     *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  */
+            
+        }
+        if(State == Debug2){
+            /* debug command free  */
+            
+        }
+        if(State == StartS){
+            /*game start...*/ 
+           return;
+        }
+        
+    }     
     
 }
 int main() {
@@ -450,9 +558,9 @@
     ControlRobo[12] = GoHome;
     
     
-    SetUp(&CompassDef);
+    SetUp(&CompassDef);/*set up routine*/
     
-    StartLoop();
+    StartLoop(); /*loop stat, switch push end*/
     
     while(1) {