Diff: movement/movement.cpp

Revision:

6:26724c287387

Parent:

4:317c53a674fa

Child:

7:44ce34007499

--- a/movement/movement.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/movement/movement.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -11,10 +11,12 @@
 
 #define PI 3.141592
 
-char ashi_data[4]={0};
+char can_ashileddata[1]= {0};
+int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
 
+Ec EC1(p22,p21,NC,500,0.05);
 Ec EC2(p26,p8,NC,500,0.05);
-Ec EC1(p22,p21,NC,500,0.05);
+
 Ticker ec_ticker;  //ec角速度計算用ticker
 
 R1370P gyro(p28,p27);
@@ -40,25 +42,26 @@
  *info.nowY.usw → 超音波センサーにより求めた機体位置のy座標
 */
 
-typedef struct{ //使用センサーの種類
+typedef struct { //使用センサーの種類
     double usw;  //超音波センサー
     double enc;  //エンコーダ
     double gyro; //ジャイロ
     //double line;//ラインセンサー
-}robo_sensor;
+} robo_sensor;
 
-typedef struct{ //機体情報の種類
+typedef struct { //機体情報の種類
     robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも
     robo_sensor nowX;
     robo_sensor nowY;
-}robo_data;
+} robo_data;
 
-robo_data info={{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
+robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-void UserLoopSetting_sensor(){
-    
+void UserLoopSetting_sensor()
+{
+
     gyro.initialize();
     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
     EC1.setDiameter_mm(25.5);
@@ -91,40 +94,43 @@
         else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
         else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
         else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
-      } else {
+    } else {
         output(FL,BL,BR,FR);
     }
 }
 
-void ashi_led(){
-    
-    if(now_angle > -1 && now_angle < 1){
-        ashi_data[0] = 1;
-    }else{
-        ashi_data[0] = 0;
+void ashi_led()
+{
+
+    if(now_angle > -1 && now_angle < 1) {
+        can_ashileddata0_0 = 1;
+    } else {
+        can_ashileddata0_0 = 0;
     }
-    
-    if(now_angle > 350){
-        ashi_data[1] = 1;
-    }else{
-        ashi_data[1] = 0;
+
+    if(now_angle > 350) {
+        can_ashileddata0_1 = 1;
+    } else {
+        can_ashileddata0_1 = 0;
     }
-    
-    if(now_x > -1 && now_x < 1){
-        
-        ashi_data[2] = 1;
-    }else{
-        ashi_data[2] = 0;
+
+    if(now_x > -1 && now_x < 1) {
+        can_ashileddata0_2 = 1;
+    } else {
+        can_ashileddata0_2 = 0;
     }
-    
-    if(now_y > -1 && now_y < 1){
-        ashi_data[3] = 1;
-    }else{
-        ashi_data[3] = 0;
+
+    if(now_y > -1 && now_y < 1) {
+        can_ashileddata0_3 = 1;
+    } else {
+        can_ashileddata0_3 = 0;
     }
+
+    can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4);
 }
 
-void calc_gyro(){
+void calc_gyro()
+{
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
 }
 
@@ -145,109 +151,112 @@
     info.nowY.enc = info.nowY.enc - d_y;  //微小時間毎に座標に加算
 }
 
-void set_cond(int t, int px, double bx, int py, double by){  //超音波センサーを使用するときの条件設定関数
+void set_cond(int t, int px, double bx, int py, double by)   //超音波センサーを使用するときの条件設定関数
+{
 //引数の詳細は関数"calc_xy_usw"参照
-  
-   xy_type = t;
-   
-   pm_typeX = px;
-   x_base = bx;
-   
-   pm_typeY = py;
-   y_base = by;   
+
+    xy_type = t;
+
+    pm_typeX = px;
+    x_base = bx;
+
+    pm_typeY = py;
+    y_base = by;
 }
 
-void calc_xy_usw(double tgt_angle){  //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+void calc_xy_usw(double tgt_angle)   //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+{
 //tgt_angle:機体の目標角度（運動初期角度と同じ/今大会では0,90,180のみ）
 //xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
 //pm_typeX,pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
 //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
-    
+
     double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
     double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
-    
+
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
-    
-    if(tgt_angle==0){
-      if((xy_type==0 || xy_type==2) && pm_typeX==0){
-        
-        info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-           
-      }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-         
-        info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-          
-      }
-      if((xy_type==1 || xy_type==2) && pm_typeY==0){
-         
-        info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-          
-      }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-         
-        info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-          
-      }
-    
-    }else if(tgt_angle==90){
-      if((xy_type==0 || xy_type==2) && pm_typeX==0){
-        
-        info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-           
-      }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-         
-        info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-          
-      }
-      if((xy_type==1 || xy_type==2) && pm_typeY==0){
-         
-        info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-          
-      }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-         
-        info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-          
-      }
-        
-    }else if(tgt_angle==180){    
-       if((xy_type==0 || xy_type==2) && pm_typeX==0){
-        
-        info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-           
-      }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-         
-        info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-          
-      }
-      if((xy_type==1 || xy_type==2) && pm_typeY==0){
-         
-        info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-          
-      }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-         
-        info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-          
-      }
+
+    if(tgt_angle==0) {
+        if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+            info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+        } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+            info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+        }
+        if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+            info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+        } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+            info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+        }
+
+    } else if(tgt_angle==90) {
+        if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+            info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+        } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+            info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+        }
+        if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+            info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+        } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+            info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+        }
+
+    } else if(tgt_angle==180) {
+        if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+            info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+        } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+            info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+        }
+        if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+            info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+        } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+            info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+        }
     }
 }
 
-void calc_xy(double target_angle, double u,double v){
+void calc_xy(double target_angle, double u,double v)
+{
 //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する
-    
-   calc_xy_enc();
-   
-   if(u != 1 || v != 1){
-      calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
-   }
-    
+
+    calc_xy_enc();
+
+    if(u != 1 || v != 1) {
+        calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
+    }
+
     now_x = u * info.nowX.enc + (1-u) * info.nowX.usw;
     now_y = v * info.nowY.enc + (1-v) * info.nowY.usw;
-    
+
     /*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){  //スタート時の0合わせ用
         ec_led = 1;
     }else{
         ec_led = 0;
     }
-    
+
     if(now_angle >-0.5 && now_angle <0.5){
         gyro_led = 1;
     }else{
@@ -391,12 +400,12 @@
 
         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);  //m1～m4に代入
         //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
-        
+
         if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
 
         MaxonControl(m1,m2,m3,m4);  //出力
         debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
-        
+
         if(t == (90/theta))break;
         if(id1_value[0] != 1)break;
     }
@@ -417,7 +426,7 @@
     r_setPDparam(r_p,r_d);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
     set_r_out(r_out_max);  //旋回時の最大出力値設定関数
     set_target_angle(target_angle);  //機体目標角度設定関数
-    
+
     while (1) {
 
         calc_xy(target_angle,u,v);
@@ -430,50 +439,51 @@
 
         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);
         //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4);
-        
+
         MaxonControl(m1,m2,m3,m4);
         debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
 
         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
-        if(id1_value[0] != 1)break; 
+        if(id1_value[0] != 1)break;
     }
 }
 
-void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v){  //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
-    
+void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v)   //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
+{
+
     double r, R=10;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
     double out;
-   
+
     calc_xy(tgt_angle, u, v);
- 
-    while(1){  //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+    while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
         gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,5,0.1,10,0.1,500,tgt_angle);
         MaxonControl(0,0,0,0);
-    
+
         calc_xy(tgt_angle, u, v);
-    
+
         r=hypot(now_x - tgt_x, now_y - tgt_y);
-    
+
         if(r < R) break;
-    
+
     }
-   
-    while(1){
-    
+
+    while(1) {
+
         calc_gyro();
-       
+
         out = 10 * (tgt_angle - now_angle);
- 
+
         if(out > 300) {  //0～179°のときは時計回りに回転
             MaxonControl(300,300,300,300);
-        }else if(out < -300){
-            MaxonControl(-300,-300,-300,-300);   
-        }else if(out <= 300 && out > -300) {
+        } else if(out < -300) {
+            MaxonControl(-300,-300,-300,-300);
+        } else if(out <= 300 && out > -300) {
             MaxonControl(out,out,out,out);
         }
- 
+
         if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
-        
+
     }
     MaxonControl(0,0,0,0);
 }
\ No newline at end of file