Diff: main.cpp

Revision:

1:e3248f278663

Parent:

0:d35efbf4d62e

Child:

2:39135c67083d

--- a/main.cpp	Sat Jan 24 05:41:05 2015 +0000
+++ b/main.cpp	Tue Jan 27 14:03:48 2015 +0000
@@ -31,7 +31,9 @@
  *
  *  p27,p28 >> I2C  >> DebugLCD
  *
- *  p21 >>  ServoMotor
+ *  p21 >>  Kick
+ *
+ *  p22 >>  ServoMotor
  *
  ******************************/
 
@@ -48,54 +50,17 @@
 
 
 #include "mbed.h"
-#include "L6470.h"
 #include "PID.h"
 #include "AQM0802A.h"
 #include "HMC6352.h"
 #include "Servo.h"
 
+#include "def.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};
@@ -103,19 +68,21 @@
 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);
+Servo       Servo(p21);
 
 Serial      Motor(p13,p14);//tx,rx
 Serial      pc(USBTX,USBRX);
 
-DigitalOut  Kick(p21);
+DigitalOut  Kick(p22);
 
 extern string StringFIN;
 extern void array(int,int,int,int);
 
-int speed[4] = {0};            
+int speed[4] = {0};
+
+/*Ir*/
+uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
 
 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
 
@@ -253,7 +220,7 @@
     
     if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
         if((data_r[0] == 0)&&(data_l[0] == 0)){
-            return 12<<8+255;
+            return (12<<8)+255;
         }
         if(data_r[0] == 0){
             return (data_l[0]/6+6)<<8 + data_l[1];
@@ -314,25 +281,13 @@
 
 /*モーター駆動処理↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/
 
-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[0] = -vr;
+    pwm[1] = 0;
     pwm[2] = 0;
-    pwm[3] = 0;
+    pwm[3] = vl;
 
     for(i = 0; i < 4; i++){
             if(pwm[i] > 100){
@@ -345,7 +300,9 @@
 }
 
 void ControlRobo0(int *CompassDef)//LeftFront   11時
-{}
+{
+    
+}
 void ControlRobo1(int *CompassDef)//Front   12時
 {
 /*前にボールがある場合の動作*/
@@ -443,12 +400,12 @@
         move(0,0);
         return;
     }
-    Step.GoHome();
+    
     /*irデータ取得 1,2位の場所、一位の距離*/
     if(!(Ir[0]%13 ==2)){
         return;
     }
-    Step.BusyWait(0);
+   
     
     if(Ir[1]>=100){//近い場合
         //2位を含んだ計算
@@ -459,7 +416,7 @@
     
     /*2位を含んだ計算*/
     /*計算された分stepmove*/
-    Step.BusyWait(0);
+   
     move(20,20);
     
     
@@ -493,12 +450,12 @@
         move(0,0);
         return;
     }
-    Step.GoHome();
+    
     /*irデータ取得 1,2位の場所、一位の距離*/
     if(!(Ir[0]%13 ==3)){
         return;
     }
-    Step.BusyWait(0);
+   
     
     if(Ir[1] >= 100){
         /*2位を含めた計算*/
@@ -508,7 +465,7 @@
     }
     /*2位を含めた計算*/
     /*Stepsetting 距離等により角度が変わる*/
-    Step.BusyWait(0);
+   
     move(-20,-20);
     /*ir,line check*/
     do{
@@ -527,7 +484,6 @@
 {
     int a = 50;
     unsigned int  ir_sm;
-    int kakudo = -90;
     double value = 0;
     double v = 50;/* cm/s */
     double x = 20;/*モーターを回す割合,今は適当*/
@@ -541,8 +497,8 @@
     value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v;
     
     if(300 - ir_sm&0x00FF >200){
-        Step.Step(-10);//適当
-        Step.BusyWait(0);   
+        //Step.Step(-10);//適当
+          
     }
     
     /*SetPerm()
@@ -550,7 +506,7 @@
      *ACCまたはMAX_SPEEDをいじればいい気がする
      *
      */
-    Step.Step(kakudo+10);
+    //Step.Step(kakudo+10);
     
     /*x = v*何か*/
     move(-x,-x);
@@ -583,7 +539,7 @@
     if(Ir[1] >=100){//近い
         /*計算*/
         
-        Servo.position(H1);
+        Servo.position(ONE);
         move(-20,-20);
         /*Step.setpaaa
           Step.move();-180+初期位置
@@ -605,7 +561,7 @@
     
         /*計算*/
         
-        Servo.position(H4);
+        Servo.position(FOUR);
         
         move(-20,-20);
         
@@ -660,13 +616,21 @@
     
     }
 void ControlRobo8(int *CompassDef)//BackLeft    7時
-{}
+{
+    Servo.position(FOUR);    
+}
 void ControlRobo9(int *CompassDef)//LeftBack    8時
-{}
+{
+    Servo.position(FIVE);    
+}
 void ControlRobo10(int *CompassDef)//Left       9時
-{}
+{
+    Servo.position(SIX);    
+}
 void ControlRobo11(int *CompassDef)//LeftFront  10時
-{}
+{
+    Servo.position(NINE);    
+}
 void GoHome(int *CompassDef)//Ball is not found.
 {
     /*line検知をしないバージョン*/
@@ -681,8 +645,8 @@
         }
         /*return;してもいいかもしれない*/
     }
-    Step.GoHome();
-    Step.BusyWait(0);
+    
+   
      
     PingReceiveFB(ping);
     if(ping[1] >=60){//後ろからの距離60cm
@@ -739,20 +703,20 @@
     Motor.baud(115200);                             //ボーレート設定
     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
-    move(0,0);//停止
+    //move(0,0);//停止
 
     //Step.Resets();
     //Step.ReleseSW(0,1);
     
-    Servo.calibrate(0.0005, 120.0);
+    Servo.calibrate(0.0006, 120.0);
     Servo.position(HOME);//タイヤを前に向ける
     
-
+/*
     hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     *compass_def = (hmc6352.sample() / 10);
     
     Lcd.cls();
-    
+  */  
     /*初期化終了*/
     
     
@@ -816,10 +780,64 @@
     }     
     
 }
+
+
+void ClockPointing(int to_st, int power){
+    
+    /*
+        to_st   時計において何時の方向にサーボを動かしたいか
+        power   タイヤのモータのパワー．絶対値．
+        
+        from_stで現在のサーボ状態保存．（初期がHOMEであること前提）
+        これを利用して，３時⇔９時の移動のみサーボを動かさずにタイヤの駆動方向の反転のみで状態遷移可能．
+        
+        改善の余地としてはワンショットタイマーを加えて
+        割り込み関数が作動するまでサーボ駆動時のタイヤの高出力を控え，
+        割り込み関数で引数の通りの出力でタイヤを駆動するというものがある．
+        しかし場合によってはサーボの移動距離が短いときネックになることがある．
+        使うときはサーボの移動距離に応じて処理を変えるなどの工夫が必要になる．
+    */
+    static int from_st = TWELFTH;
+    static int qb=0;
+    
+    int TireDir[]  = {1, 1, 1, -1, -1, -1, -1, -1, 1, 1, 1, 1};//１時、２時、３時、４時…１２時//タイヤの駆動方向
+    float ClockDir[]={ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN, EIGHT, NINE, TEN, ELEVEN, TWELVE};//サーボの方向
+    
+    //想定外の引数が来た時の対処
+    if(
+        (to_st > TWELFTH)
+    ||  (to_st < FIRST)
+    
+    ||  (from_st > TWELFTH)
+    ||  (from_st < FIRST)
+    
+    )   return;
+    
+    
+    if(
+        (from_st == THIRD  ||  from_st == NINTH)
+    &&  (to_st == THIRD ||  to_st == NINTH)
+    &&  (from_st != to_st)
+    ){
+        qb = !qb;
+        if(qb) for(int i=0; i<12; i++) TireDir[i] = TireDir[i]*(-1);//タイヤの駆動方向反転
+    }
+    else{
+        qb=0;
+        Servo.position(ClockDir[to_st]);//サーボの方向転換
+    }
+    
+    move(abs(power)*TireDir[to_st], abs(power)*TireDir[to_st]);//タイヤの出力
+    
+    from_st = to_st;//状態（サーボの）のプログラム上の遷移
+    
+}
+
 int main() {
     
     /*Ir*/
-    uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
+    //uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
+    //→global変数
     
     /*Line*/
     uint8_t Line;
@@ -852,13 +870,13 @@
     
     SetUp(&CompassDef);/*set up routine*/
     
-    StartLoop(); /*loop stat, switch push end*/
+    //StartLoop(); /*loop stat, switch push end*/
     
     /*
     前を向くように設定をするべき
     アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
     */
-    while(1){//前を向かせるwhile
+    while(0){//前を向かせるwhile
         Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
         if(Compass == 3) break;//前を向いたら抜ける。
     }
@@ -879,10 +897,6 @@
         
         }
         
-        if(Step.BusyCheck()){//ステッピングが動いていないか
-            continue;
-            
-        }
         
         IrNum = IrReceiveS();
         
@@ -891,35 +905,52 @@
         //wait_ms(0);
     }
     
+    
+    move(0,0);
+    Servo.position(HOME);
+    uint8_t clock[]={TWELFTH, THIRD, SIXTH, NINTH};
+    uint8_t clock2[]={FIRST, SECOND, THIRD, FOURTH, FIFTH, SIXTH, SEVENTH, EIGHTH, NINTH, TENTH, ELEVENTH, TWELFTH};
+    Servo.position(HOME);
     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);
         
     
+        /*デモプログラム
+         *
+         *正方形の軌道
+         *
+         *円軌道
+         *
+         *反復横跳び
+         *
+         *関数ClockPointingを活用することで少しだけ楽に以上の動きを実現できる．
+         */
+        for(int i=0; i < 4; i++){
+            
+            ClockPointing(clock[i], 20);
+            
+            wait(2);
+            
+        }
         
+        for(int i=0; i < 12; i++){
+            
+            ClockPointing(clock2[i], 20);
+            
+            wait(1);
+            
+        }
+        
+        for(int i=0; i < 4; i++){
+            
+            ClockPointing(THIRD, 20);
+                
+            wait(2);
+                
+            ClockPointing(NINTH, 20);
+                
+            wait(2);
+        }
+
     }
     
 }