Diff: main.cpp

Revision:

10:ebb59c1d369e

Parent:

9:ce5a1315fe0d

Child:

11:b89289eabaa2

--- a/main.cpp	Sun Aug 26 09:07:19 2018 +0000
+++ b/main.cpp	Tue Sep 11 02:51:23 2018 +0000
@@ -15,8 +15,8 @@
 elements getRobotVelocity(state);
 state getTargetState();
 int updateStateNum();
-bool isConvergetnceTops(int);
-
+bool isConvergenceTops(int);
+bool isConvergenceSupply(int);
 controller getPropoData();
 
 //x軸補正用 PID
@@ -45,10 +45,11 @@
     imu.performCalibration();
     imu.startAngleComputing();
     
-    uss.startTriger();
+    uss[0].startTriger();
+    uss[1].startTriger();
     
     enc[0].changeDirection();
-    enc[1].changeDirection();
+    //enc[1].changeDirection();
     enc[2].changeDirection();
 
     //オドメーターの定義
@@ -79,7 +80,7 @@
     pidRobotYow.sensor = &imu.angle[2];
     pidRobotYow.target = &target_yow;
     pidRobotYow.start();
-    pidUss.sensor = &uss.distance;
+    pidUss.sensor = &uss[0].distance;
     pidUss.target = &target_uss;
     pidUss.start();
     
@@ -91,9 +92,9 @@
     timer.start();
     
     while(1)
-    {
-        //controller cmd = getPropoData();   //getPropoData & getCanData
-        //float robot_velocity[3] = {cmd.LX, cmd.LY, cmd.RX};
+    {   
+    
+        controller cmd = getPropoData();   //getPropoData & getCanData
         
         can.read();
         
@@ -111,20 +112,13 @@
         double now_time = timer.read();
         if(now_time - pre_time >= 0.01)
         {
-            while(can.send() == 0);
-            pre_time = now_time;
+            if(can.send())
+                pre_time = now_time;
         }
-        
-        if(sw2 == 1)
-        {
-            odm.x = 0;
-            odm.y = 0;
-            imu.angle[2] = 0;
-        }
-        
         //ロボット速度を取得
         elements robot_vel = getRobotVelocity(tar_state);
-        float robot_velocity[3] = {robot_vel.x, robot_vel.y, robot_vel.theta};
+        //float robot_velocity[3] = {robot_vel.x, robot_vel.y, robot_vel.theta};
+        float robot_velocity[3] = {cmd.LX, cmd.LY, cmd.RX};
         
         //ホイール速度計算
         mecanum.setVelG(robot_velocity);
@@ -134,26 +128,16 @@
         //モーターの駆動
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
-        
-        //pc.printf("%.2f\t", enc[0].rotations);
-        
-        pc.printf("%.2f\t", odm.x);
-        pc.printf("%.2f\t", odm.y);
+            //Motor[i].drive(0.5);
+    
         pc.printf("%.2f\t", imu.angle[2]);
-        pc.printf("%.2f\t", uss.distance);
-        pc.printf("%.2f\t", target_yow);
-        pc.printf("%.2f\t", pidRobotYow.target);
-        pc.printf("%d\t", can.get(4, 1));
-        
+        pc.printf("%.2f\t", uss[0].distance);
+        pc.printf("%.2f\t", uss[1].distance);
         
-        //pc.printf("%.2f\t", enc[0].rotations);
-        /*
-        pc.printf("%.2f\t", tar_state.y);
-        pc.printf("%.2f\t", tar_state.theta);
-        pc.printf("%.2f\t", tar_state.shoot);
-        pc.printf("%.2f\t", tar_state.angle);
-        pc.printf("%.2f\t", tar_state.supply);
-        */
+        pc.printf("%.2f\t", enc[0].rotations);
+        pc.printf("%.2f\t", enc[1].rotations);
+        pc.printf("%.2f\t", enc[2].rotations);
+           
         pc.printf("\n");
     }
 }
@@ -207,7 +191,7 @@
     //超音波で近づくとき
     if(state_lib[num][1] >= 10)
     {
-        if(pidUss.isConvergence(t+1) == 1 && isConvergetnceTops(num) == 1)
+        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
             flag_y = 1;        
     }
     else if(pidRobotY.isConvergence(t) == 1)
@@ -215,7 +199,7 @@
     
     if(state_lib[num][0] >= 10)
     {
-        if(pidUss.isConvergence(t+1) == 1 && isConvergetnceTops(num) == 1)
+        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
             flag_x = 1;
     }
     else if(pidRobotX.isConvergence(t) == 1)
@@ -270,19 +254,29 @@
     
     return vel;
 }
-bool isConvergetnceTops(int state_num)
+bool isConvergenceTops(int state_num)
 {
     int velocity_pid = can.get(3, 1);
     int angle_pid    = can.get(3, 2);
     int velocity_val = can.get(3, 3);
     float angle_val    = can.get(3, 4);
-    
     angle_val /= 2.0;
+        
     if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_lib[state_num][3] && angle_val == state_lib[state_num][4])
         return 1;
     else return 0;
 }
-
+bool isConvergenceSupply(int state_num)
+{
+    int is_supply_done = can.get(3, 5);
+        
+    if(state_lib[state_num][5] == 1)
+    {
+        if(is_supply_done == 1)
+            return 1;
+        else return 0;
+    }else return 1;
+}
 controller getPropoData()
 {
     float dead_zone = 0.05;
@@ -302,8 +296,8 @@
         propo.RX = -sbus.getStickVal(2) / 255.0;
         propo.RY = sbus.getStickVal(3) / 255.0;
         propo.H = sbus.getSwitchVal(0);
-        propo.A = sbus.getSwitchVal(1);
-        propo.D = sbus.getSwitchVal(2);
+        propo.C = sbus.getSwitchVal(1);
+        propo.E = sbus.getSwitchVal(2);
         propo.F = sbus.getSwitchVal(3);
         propo.G = sbus.getSwitchVal(4);
     }
@@ -313,4 +307,4 @@
     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;
-}
+}
\ No newline at end of file