Diff: main.cpp

Revision:

0:b811f40948e2

Child:

1:2189d6238ac7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 06 02:30:36 2018 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "SpeedController.h"
+#include "EC.h"
+#include "R1370P.h"
+#include "enc_1multi.h"
+#include "math.h"
+#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる
+#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);//モーター定義
+
+Ec EC1(NC,NC,NC,300,0.05);
+Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義
+
+Ticker motor_tick;  //角速度計算用ticker
+Ticker ticker;//for enc
+
+Serial pc(USBTX, USBRX); // tx, rx //PC USB
+R1370P gyro(PC_6,PC_7);
+
+void calOmega() //角速度計算関数
+{
+    Motor_RF.CalOmega();
+    Motor_LF.CalOmega();
+    Motor_LB.CalOmega();
+    Motor_RB.CalOmega();
+    EC1.CalOmega();
+    EC2.CalOmega();
+}
+double new_dist1=0,new_dist2=0;
+double old_dist1=0,old_dist2=0;
+double d_dist1=0,d_dist2=0;//座標計算用関数
+double now_x;//現在地X座標
+double now_y;//現在地Y座標
+double now_angle; //現在角度
+
+double start_x=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)//各モーターの動作明瞭化
+{
+    target_RF=BASIC_SPEED*iRF;
+    target_LF=BASIC_SPEED*iLF;
+    target_LB=BASIC_SPEED*iLB;
+    target_RB=BASIC_SPEED*iRB;
+}
+
+
+int main()
+{
+    gyro.initialize();    //main関数の最初に一度だけ実行
+    gyro.acc_offset();    //やってもやらなくてもいい
+    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);//測定輪半径
+    double  getDistance_mm();
+
+    void reset();
+    EC1.reset();
+
+    now_x=start_x;
+    now_y=start_y;
+    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;//微小時間当たりのエンコーダ読み込み
+
+
+        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);//微小時間毎の座標変化
+
+
+        now_x=now_x+d_x;
+        now_y=now_y+d_y;//微小時間毎に座標に加算
+    }
+}