春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_main_ver5

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed EC PathFollowing-ver11 CruizCore_R1370P

Files at this revision

API Documentation at this revision

Revision 3:e696a6dd4254, committed 2018-11-24

Comitter:
yuki0701
Date:
Sat Nov 24 14:45:01 2018 +0000
Parent:
2:e04e6b5d6584
Child:
4:65dbc0080845
Commit message:

Changed in this revision

PathFollowing.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
move4wheel.lib Show annotated file Show diff for this revision Revisions of this file 
--- a/PathFollowing.lib	Sat Nov 17 05:35:23 2018 +0000
+++ b/PathFollowing.lib	Sat Nov 24 14:45:01 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/F3RC4/code/PathFollowing/#591749f315ac
+https://os.mbed.com/teams/F3RC4/code/PathFollowing/#d438093cb464

--- 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;

--- a/move4wheel.lib	Sat Nov 17 05:35:23 2018 +0000
+++ b/move4wheel.lib	Sat Nov 24 14:45:01 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/F3RC4/code/move4wheel2/#ae8974b90ca7
+https://os.mbed.com/teams/F3RC4/code/move4wheel2/#8da9e67ac000