Diff: main.cpp

Revision:

1:2189d6238ac7

Parent:

0:b811f40948e2

diff -r b811f40948e2 -r 2189d6238ac7 main.cpp
--- a/main.cpp	Sat Oct 06 02:30:36 2018 +0000
+++ b/main.cpp	Fri Oct 26 05:13:04 2018 +0000
@@ -1,17 +1,17 @@
 #include "mbed.h"
-#include "SpeedController.h"
-#include "EC.h"
-#include "R1370P.h"
-#include "enc_1multi.h"
+#include "SpeedController.h"//最終的にはSpeedController使わないけど
+#include "EC.h"//エンコーダ
+#include "R1370P.h"//ジャイロ
+//#include "enc_1multi.h"
 #include "math.h"
-#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる
+#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる//SpeedController
 #define PI 3.141592
 
 
 SpeedControl Motor_RF(NC,NC,NC,500,0.05,NC,NC);
 SpeedControl Motor_LF(NC,NC,NC,500,0.05,NC,NC);
 SpeedControl Motor_LB(NC,NC,NC,500,0.05,NC,NC);
-SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義
+SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義//SpeedController
 
 Ec EC1(NC,NC,NC,300,0.05);
 Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義
@@ -27,7 +27,7 @@
     Motor_RF.CalOmega();
     Motor_LF.CalOmega();
     Motor_LB.CalOmega();
-    Motor_RB.CalOmega();
+    Motor_RB.CalOmega();//SpeedController
     EC1.CalOmega();
     EC2.CalOmega();
 }
@@ -39,40 +39,80 @@
 double now_angle; //現在角度
 
 double start_x=0;
-double start_y=0;
+double start_y=0;//起点
 
 double target_RF=0,target_LF=0,target_LB=0,target_RB=0;//目標速度
 Timer t;
-int i=0;//い　つ　も　の
 
 
 
-void idou(double iRF,double iLF,double iLB,double iRB)//各モーターの動作明瞭化
+
+void output(double iRF,double iLF,double iLB,double iRB)//後にSPI用に変える
 {
     target_RF=BASIC_SPEED*iRF;
     target_LF=BASIC_SPEED*iLF;
     target_LB=BASIC_SPEED*iLB;
     target_RB=BASIC_SPEED*iRB;
 }
+void base(double bRF,double bLF,double bLB,double bRB,double Max)//いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
+{
+    
+    if     (fabs(bRF)>=fabs(bLF)&&fabs(bRF)>=fabs(bLB)&&fabs(bRF)>=fabs(bRB))output(Max              ,Max*bLF/fabs(bRF),Max*bLB/fabs(bRF),Max*bRB/fabs(bRF));
+    else if(fabs(bLF)>=fabs(bRF)&&fabs(bLF)>=fabs(bLB)&&fabs(bLF)>=fabs(bRB))output(Max*bRF/fabs(bLF),Max              ,Max*bLB/fabs(bLF),Max*bRB/fabs(bLF));
+    else if(fabs(bLB)>=fabs(bRF)&&fabs(bLB)>=fabs(bLF)&&fabs(bLB)>=fabs(bRB))output(Max*bRF/fabs(bLB),Max*bLF/fabs(bLB),Max              ,Max*bRB/fabs(bLB));
+    else                                                                     output(Max*bRF/fabs(bRB),Max*bLF/fabs(bRB),Max*bLB/fabs(bRB),Max              );
+}
 
 
+
+//ここからそれぞれのプログラム//////////////////////////////////////////////////////
+//now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
+//ジャイロの出力は角度だが三角関数はラジアンとして計算する
+//通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
+//ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
+
+void gostraight(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;
+    }
+}
+
+//ここまで///////////////////////////////////////////////////////////////////////
+
 int main()
 {
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();    //やってもやらなくてもいい
-    printf("start\r\n");
+    //printf("start\r\n");
 
     motor_tick.attach(&calOmega,0.05);
-    Motor_RF.setDOconstant(30);
-    Motor_LF.setDOconstant(30);//c
-    Motor_LB.setDOconstant(30);
-    Motor_RB.setDOconstant(30);//c
-
-    Motor_RF.setPDparam(0,0);
-    Motor_LF.setPDparam(0,0);//pd
-    Motor_LB.setPDparam(0,0);
-    Motor_RB.setPDparam(0,0);//pd
-
 
     EC1.setDiameter_mm(48);
     EC2.setDiameter_mm(48);//測定輪半径
@@ -83,6 +123,7 @@
 
     now_x=start_x;
     now_y=start_y;
+
     while(1) {
         now_angle=gyro.getAngle();
 
@@ -94,8 +135,8 @@
         old_dist2=new_dist2;//微小時間当たりのエンコーダ読み込み
 
 
-        double d_x=d_dist2*sin(now_angle*3.1415926535/180)-d_dist1*sin(now_angle*3.1415926535/180);
-        double d_y=d_dist2*sin(now_angle*3.1415926535/180)+d_dist1*cos(now_angle*3.1415926535/180);//微小時間毎の座標変化
+        double d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*sin(now_angle*PI/180);
+        double d_y=d_dist2*sin(now_angle*PI/180)+d_dist1*cos(now_angle*PI/180);//微小時間毎の座標変化
 
 
         now_x=now_x+d_x;