Diff: main.cpp

Revision:

0:6db16ad02a1b

Child:

1:d7ceb38da3d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Aug 04 13:07:23 2018 +0000
@@ -0,0 +1,266 @@
+#include "mbed.h"
+#include "DriveController.h"
+#include "IMU.h"
+#include "MDD.h"
+#include "MDD2.h"
+#include "Mycan.h"
+#include "Odometer.h"
+#include "PID.h"
+#include "RotaryEncoder.h"
+#include "CSV.h"
+#include "SBUS.h"
+#include "USS.h"
+#include "hardwareConfig.h"
+
+//(X, Y, θ, speed, angle, injection, 補給昇降)
+float position[10][7] = {{0.0, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化
+                         {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動
+                         {-0.3, 5.0, 0.0, 0, 90, 0, 0.0},//発射
+                         {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動,角度戻す, P上昇
+                         
+                         {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
+                         {-0.6, 5.0, 0.0, 0, 90, 0, 0.0},//y移動
+                         {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//発射
+                         //{6.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
+                         
+                         {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
+                         {-0.9, 5.0, 0.0, 20, 90, 0, 0.0},//y移動
+                         {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//発射
+                         //{7.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
+                         /*
+                         {3.0, 0.0, 0.0, 20, 80, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
+                         {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//y移動
+                         {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//下段に発射
+                         {3.0, 2.5, 0.0, 20, 90, 0, 6.8},//角度戻す, P上昇
+                         {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//角度変化, 下降&バットマン駆動
+                         {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//上段に発射
+                         {0.0, 0.0, 0.0, 20, 90, 0, 6.8} //x・y移動,スタートゾーンに戻る
+                         */
+                        };
+
+controller getPropoData();
+//bool isConvergence(int num);
+
+//x軸補正用 PID
+PID pidRobotX(2, 0, 0, 0.01, 0.3);
+float target_x = 0;
+
+//y軸補正用 PID
+PID pidRobotY(2, 0, 0, 0.01, 0.3);
+float target_y = 0;
+
+//USS用
+PID pidUss(0.02, 0, 0, 0.01, 0.3);
+float target_uss = 25.0;
+
+int main()
+{
+    //タイマー３の優先度を最低にする
+    NVIC_SetPriority(TIMER3_IRQn, 100);
+
+    //IMUのキャリブレーション
+    imu.performCalibration();
+    imu.startAngleComputing();
+
+    //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
+    PID pidRobotYow(0.05, 0, 0, 0.01, 0.95);
+    float target = 0;
+    mecanum.imu_yow = &imu.angle[2];
+    pidRobotYow.sensor = &imu.angle[2];
+    pidRobotYow.target = ⌖
+    pidRobotYow.start();
+
+    for(int i; i < 3; i++)
+        enc[i].changeDirection();
+
+    //オドメーターの定義
+    float matrix[3][3] = {{1, 0, 0},
+                          {0, 1, 0},
+                          {0, 0, 0}
+                         };
+    Odometer odm(matrix, 0.048);
+    float tmp = 0;
+    float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
+    odm.setupOdometerSensors(encoders, &imu.angle[2]);
+    odm.startComputingOdometry(0.005, 0, 0, 0);
+
+    //オドメーターX
+    pidRobotX.sensor = &odm.x;
+    pidRobotX.target = &target_x;
+    pidRobotX.start();
+
+    //オドメーターY
+    pidRobotY.sensor = &odm.y;
+    pidRobotY.target = &target_y;
+    pidRobotY.start();
+
+    //USS  pid設定
+    uss.startTriger();
+    pidUss.sensor = &uss.distance;
+    pidUss.target = &target_uss;
+    pidUss.start();
+
+    while(1) 
+    {
+        controller cmd = getPropoData();   //getPropoData & getCanData
+        
+        static int posi_num = 0;
+        static int reset_swich = 0;
+        if(cmd.H == 2 && reset_swich == 0)
+        {
+            posi_num += 1;
+            if(posi_num > 9)
+                posi_num = 0;
+            reset_swich = 1;
+        }
+        else if(cmd.H == 0 && reset_swich == 1)
+            reset_swich = 0;
+
+        if(position[posi_num][5] == 1)
+            can.set(1, 1, position[posi_num][3]);
+        else
+            can.set(1, 1, 0);
+        can.set(1, 2, position[posi_num][4]);
+        can.set(2, 1, position[posi_num][6]);
+        can.send();
+        
+        //ロボットの移動速度(LX, LY, RX)
+        float robot_velocity[3];
+
+        //yow角調整処理
+        *pidRobotYow.target = position[posi_num][2];
+        robot_velocity[2] = pidRobotYow.output;
+
+        //x軸調整処理
+        *pidRobotX.target = position[posi_num][0];
+        robot_velocity[0] = pidRobotX.output;
+        
+        //USS距離調整処理
+        if(posi_num == 2 || posi_num == 5 || posi_num == 8)
+        {
+            *pidUss.target = position[posi_num][1];
+            robot_velocity[1] = -pidUss.output;
+        }
+        else 
+        {
+            //y軸調整処理
+            *pidRobotY.target = position[posi_num][1];
+            robot_velocity[1] = pidRobotY.output;
+        }
+
+        //角度・XYリセット
+        if(cmd.F == 2) 
+        {
+            imu.angle[2] = 0;
+            *pidRobotYow.target = 0;
+            odm.x = 0;
+            *pidRobotX.target = 0;
+            odm.y = 0;
+            *pidRobotY.target = 0;
+        }
+
+        //ホイール速度計算
+        mecanum.setVelL(robot_velocity);
+        mecanum.computeWheelVel();
+        mecanum.rescaleWheelVel();
+
+        //モーターの駆動
+        for(int i = 0; i < 4; i++)
+            Motor[i].drive(mecanum.wheel_vel[i]);
+        
+        //posi_num += isConvergence(posi_num);
+        
+        pc.printf("%.2f\t", odm.x);
+        pc.printf("%.2f\t", odm.y);
+        
+        pc.printf("\n");
+        wait(0.02);
+    }
+}
+
+controller getPropoData()
+{
+    float dead_zone = 0.05;
+    controller propo;
+    sbus.isFailSafe();
+
+    //propo直接コントロール
+    if(sbus.isFailSafe()) 
+    {
+        propo.LX = propo.LY = propo.RX = propo.RY = 0;
+        propo.H = propo.A = propo.D = propo.F = propo.G = propo.fail_safe = 0;
+    } 
+    else 
+    {
+        propo.LX = sbus.getStickVal(0) / 255.0;
+        propo.LY = sbus.getStickVal(1) / 255.0;
+        propo.RX = -sbus.getStickVal(2) / 255.0;
+        propo.RY = sbus.getStickVal(3) / 255.0;
+        propo.H = sbus.getSwitchVal(0);
+        propo.C = sbus.getSwitchVal(1);
+        propo.E = sbus.getSwitchVal(2);
+        propo.F = sbus.getSwitchVal(3);
+        propo.G = sbus.getSwitchVal(4);
+        propo.fail_safe = 1;
+    }
+
+    if(propo.RX < dead_zone && propo.RX > -dead_zone)  propo.RX = 0;
+    if(propo.RY < dead_zone && propo.RY > -dead_zone)  propo.RY = 0;
+    if(propo.LX < dead_zone && propo.LX > -dead_zone)  propo.LX = 0;
+    if(propo.LY < dead_zone && propo.LY > -dead_zone)  propo.LY = 0;
+    return propo;
+}
+
+/*
+bool isConvergence(int num)
+{
+    bool tf;
+    if(num > 3 && num < 8)
+        num -= 4;
+    else if(num > 7 && num < 12)
+        num -= 8;
+    else if(num > 11 && num < 15)
+        num -= 8;
+
+    if(num == 0)
+    {
+        if(pidRobotX.output == 0)
+            tf = 1;
+        else
+            tf = 0;
+    }
+    else if(num == 1)
+    {
+        if(pidUss.output == 0)
+            tf = 1;
+        else
+            tf = 0;
+    }
+    else if(num == 2) 
+    {
+        wait(1.0);
+        tf = 1;
+    } 
+    else if(num == 3)
+    {
+        if(pidRobotY.output == 0)
+            tf = 1;
+        else
+            tf = 0;
+    }
+    else if(num == 15)
+    {
+        
+    }
+    else if(num == 16)
+    {
+        
+    }
+    else if(num == 17)
+    {
+        
+    }
+
+    return tf;
+}
+*/
\ No newline at end of file