Diff: main.cpp

Revision:

3:e696a6dd4254

Parent:

2:e04e6b5d6584

Child:

5:7493649d098b

--- a/main.cpp	Sat Nov 17 05:35:23 2018 +0000
+++ b/main.cpp	Sat Nov 24 14:45:01 2018 +0000
@@ -20,9 +20,10 @@
 #define SPI_BITS    16
 #define SPI_MODE    0
 #define SPI_WAIT_US 1               // 1us
-SPI spi(PB_5,PB_4,PB_3);
+//SPI spi(PB_5,PB_4,PB_3); //Nucleo
+SPI spi(p5,p6,p7);  //mbed
 
-DigitalOut ss_md1(PB_15);           //エスコンの設定
+/*DigitalOut ss_md1(PB_15);           //エスコンの設定
 DigitalOut ss_md2(PB_14);
 DigitalOut ss_md3(PB_13);
 DigitalOut ss_md4(PC_4);
@@ -31,7 +32,18 @@
 //DigitalIn md_ch_enable(p10);        // check enable switch is open or close
 //Timer md_disable;
 DigitalOut md_stop(PA_14);          // stop all motor
-DigitalIn  md_check(PB_7);           // check error of all motor driver  //とりあえず使わない
+DigitalIn  md_check(PB_7);           // check error of all motor driver  //とりあえず使わない*/
+
+DigitalOut ss_md1(p15);           //エスコンの設定
+DigitalOut ss_md2(p16);
+DigitalOut ss_md3(p17);
+DigitalOut ss_md4(p18);
+
+DigitalOut md_enable(p25);
+//Timer md_disable;
+DigitalOut md_stop(p24);          // stop all motor
+DigitalIn  md_check(p23);           // check error of all motor driver  //とりあえず使わない
+
 
 /*モーターの配置
 *     md1//---F---\\md4
@@ -42,12 +54,16 @@
 */
 
 
-Ec EC1(PC_6,PC_8,NC,500,0.05);
-Ec EC2(PB_1,PB_12,NC,500,0.05);  //エンコーダ
+//Ec EC1(PC_6,PC_8,NC,500,0.05);
+//Ec EC2(PB_1,PB_12,NC,500,0.05);  //Nucleo
+
+Ec EC1(p21,p22,NC,500,0.05);  
+Ec EC2(p8,p26,NC,500,0.05); //←mbad
 Ticker motor_tick;  //角速度計算用ticker
 Ticker ticker;  //for enc
 
-R1370P gyro(PC_6,PC_7);  //ジャイロ
+//R1370P gyro(PC_6,PC_7);  //ジャイロ
+R1370P gyro(p28,p27);
 
 //DigitalOut can_led(LED1);           //if can enable -> toggle
 DigitalOut debug_led(LED2);         //if debugmode -> on
@@ -61,8 +77,11 @@
 //現在地X,y座標、現在角度については、PathFollowingでnow_x,now_y,now_angleを定義済
 double start_x=0,start_y=0;  //スタート位置
 
+double x_out,y_out,r_out;//出力値
+
 static int16_t m1=0, m2=0, m3=0, m4=0;  //int16bit = int2byte
 
+///////////////////////////////////////////////////関数のプロトタイプ宣言////////////////////////////////////////////////////
 void UserLoopSetting();             // initialize setting
 void DAC_Write(int16_t data, DigitalOut* DAC_cs);
 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4);
@@ -85,10 +104,28 @@
 //いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
 //マクソンは-4095～4095だからMax=4095にする//最速スピードを出すための関数になってる
 {
-    if     (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max            ,Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL));
-    else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max            ,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            ,Max*FR/fabs(BR));
-    else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max            );
+    if     (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max*FL/fabs(FL),Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL));
+    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));
+}
+
+void calc_xy()
+{
+    now_angle=gyro.getAngle();  //ジャイロの値読み込み
+
+    new_dist1=EC1.getDistance_mm();
+    new_dist2=EC2.getDistance_mm();
+    d_dist1=new_dist1-old_dist1;
+    d_dist2=new_dist2-old_dist2;
+    old_dist1=new_dist1;
+    old_dist2=new_dist2;  //微小時間当たりのエンコーダ読み込み
+
+    d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
+    d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180);  //微小時間毎の座標変化
+    now_x=now_x+d_x;
+    now_y=now_y-d_y;  //微小時間毎に座標に加算
+    
 }
 
 //ここからそれぞれのプログラム//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -103,7 +140,7 @@
 //X=円弧の中心座標、Y=円弧の中心座標、r=円弧の半径、theta=plotの間隔(0～90°)、v=目標速度
 
     int s;
-    int t = 4;
+    int t = 0;
     double plotx[(90/theta)+1];  //円弧にとるplotのx座標
     double ploty[(90/theta)+1];
     //double plotvx[(90/theta)+1];  //各plotにおける速度
@@ -115,43 +152,33 @@
         case 1://↑から→
 
             for(s=0; s<((90/theta)+1); s++) {
-                plotx[s] = X + r * cos(PI - s * (PI*theta/180)) + r;
+                plotx[s] = X + r * cos(PI - s * (PI*theta/180));
                 ploty[s] = Y + r * sin(PI - s * (PI*theta/180));
                 //plotvx[s] = -v * cos(PI - s * (PI*theta/180));
                 //plotvy[s] = v * sin(PI - s * (PI*theta/180));
-                //printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+                //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
             }
 
             while(1) {
                 now_angle=gyro.getAngle();  //ジャイロの値読み込み
 
-                new_dist1=EC1.getDistance_mm();
-                new_dist2=EC2.getDistance_mm();
-                d_dist1=new_dist1-old_dist1;
-                d_dist2=new_dist2-old_dist2;
-                old_dist1=new_dist1;
-                old_dist2=new_dist2;  //微小時間当たりのエンコーダ読み込み
-
-                d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
-                d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180);  //微小時間毎の座標変化
-                now_x=now_x+d_x;
-                now_y=now_y+d_y;  //微小時間毎に座標に加算
+                calc_xy();
 
                 XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
                 CalMotorOut(x_out,y_out,r_out);  //move4wheel内のモーター番号定義または成分分解が違うかも？
                 //CalMotorOut(plotvx[t], plotvy[t],0);
 
-                //printf("t=%d x_out=%f y_out=%f\n\r",t,x_out,y_out);
-                printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
+                //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
+                //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
 
-                output(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));  //m1～m4に代入
+                base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);  //m1～m4に代入
 
-                if(((X - now_x)*(plotx[t+1] - plotx[t]) + (Y - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
+                if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
                 if(t == (90/theta))break;
 
                 MotorControl(m1,m2,m3,m4);  //出力
 
-                //printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f\n\r",m1,m2,m3,m4,now_x,now_y);
+                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);
 
             }
 
@@ -165,53 +192,105 @@
             while(1) {
 
                 now_angle=gyro.getAngle();  //ジャイロの値読み込み
-
-                new_dist1=EC1.getDistance_mm();
-                new_dist2=EC2.getDistance_mm();
-                d_dist1=new_dist1-old_dist1;
-                d_dist2=new_dist2-old_dist2;
-                old_dist1=new_dist1;
-                old_dist2=new_dist2;  //微小時間当たりのエンコーダ読み込み
-
-                d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180);
-                d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180);  //微小時間毎の座標変化
-                now_x=now_x+d_x;
-                now_y=now_y+d_y;  //微小時間毎に座標に加算
+                
+                calc_xy();
 
                 XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
                 CalMotorOut(x_out,y_out,r_out);
-                base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);
-                if(((X - now_x)*(plotx[t+1] - plotx[t]) + (Y - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
+                base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);
+                if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
                 if(t == (90/theta))break;
 
+                MotorControl(m1,m2,m3,m4);
             }
     }
 }
-//ここまで///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void gogo_straight(double x1_point,double y1_point,double x2_point,double y2_point)//直線運動プログラム（引数：出発地点の座標（x,y）、目標地点の座標(x,y)）
+{
+    while (1) {
+
+        //now_angle=gyro.getAngle();
+
+        calc_xy();
+        printf("x = %f, y = %f, angle = %f\r\n",now_x,now_y,now_angle);
+
+
+        //Debug_Control();
+        XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out);
+        //printf("x=%lf, y=%lf, r=%lf",x_out, y_out,r_out);
+
+        CalMotorOut(x_out,y_out,r_out);
+        //printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
+
+        base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);
+        //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m1,m2,m3,m4);
+        MotorControl(m1,m2,m3,m4);
+        
+        
+
+        if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0) break;
+
+    }
+
+    MotorControl(0,0,0,0);
+}
+
+void go_straight(int type,double goal_x,double goal_y,double speed,double front)//移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
+{
+    double y_hosei=(now_y-goal_y)*0.001;//Y座標(mm単位)にP処理
+    double x_hosei=(now_x-goal_x)*0.001;//X座標(mm単位)にP処理
+    double incl_hosei=sin(now_angle-front)*(PI/180)*0.1;//機体角度(sin(数度→ラジアンに変換))にP処理
+
+    switch(type) {
+        case 1://Y座標一定の正方向横移動
+            while(now_x<goal_x){
+            base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
+            }
+            break;
+            
+        case 2://Y座標一定の負方向横移動
+            while(now_x>goal_x){
+            base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
+            }
+            break;
+            
+        case 3://Y座標一定の正方向横移動
+            while(now_y<goal_y){
+            base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
+            }
+            break;
+            
+        case 4://X座標一定の負方向横移動
+            while(now_y>goal_y){
+            base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
+            }
+            break;
+    }
+}
+
+//////////////////////////////////////////////////////////////以下main文////////////////////////////////////////////////////////////////////////
 
 int main()
 {
     UserLoopSetting();
 
-    /*void reset();
+    void reset();
     EC1.reset();
-    EC2.reset();*/
+    EC2.reset();
 
     now_x=start_x;
     now_y=start_y;
-
-    //m1, m2, m3, m4 に出力を代入すればとりあえず動く
-
-    purecurve(1,0,0,1000,9,1000);
-
-    /*while(1) {
+  
+   
+   // purecurve(1,1000,0,1000,9,1000);
+   // MotorControl(0,0,0,0);    
+   
+    
+   //gogo_straight(0,0,1500,0);
 
-                //Debug_Control();
-
-                //MotorControl(m1,m2,m3,m4);
-
-      }*/
 }
+///////////////////////////////////////////////////////////////////////以下マクソン関連///////////////////////////////////////////////////////////////////////////
 
 void UserLoopSetting()
 {
@@ -232,11 +311,11 @@
     EC1.setDiameter_mm(48);
     EC2.setDiameter_mm(48);  //測定輪半径
 //-----PathFollowingのパラメーター設定-----//
-    set_p_out(1000);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
-    q_setPDparam(30,30);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
-    r_setPDparam(30,30);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
-    set_r_out(1000);  //旋回時の最大出力値設定関数
-    set_target_angle(0);  //機体目標角度設定関数
+    set_p_out(800);  //ベクトルABに平行方向の出力値設定関数(カーブを曲がる速度)
+    q_setPDparam(0.1,0.1);  //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
+    r_setPDparam(10,0.1);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
+    set_r_out(500);  //旋回時の最大出力値設定関数
+  //  set_target_angle(0);  //機体目標角度設定関数
 
 #ifdef DEBUG_MODE
     debug_led = 1;