Diff: main.cpp

Revision:

53:701dc7886fff

Parent:

52:f5ae47e683fa

Child:

54:a88208d9bc1d

diff -r f5ae47e683fa -r 701dc7886fff main.cpp
--- a/main.cpp	Thu Sep 14 11:56:51 2017 +0000
+++ b/main.cpp	Fri Sep 15 04:12:06 2017 +0000
@@ -93,7 +93,7 @@
 void cylinder_cal();
 void boost();
 void cylinder_origin();
-void cylinder_origin_first();
+void firstmotion();
 void sword_cal();
 void servo_origin();
 float shoulder_right_cal();
@@ -131,7 +131,7 @@
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
-//    cylinder_origin_first();
+    firstmotion();
     cylinder_pos_num = 2;                   //セットアップタイムでの初期装填のため
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
@@ -173,12 +173,30 @@
 }
 
 //cylinder
-void cylinder_origin_first()
+void firstmotion()
 {
-    while(interrupt_cylinder_min == 1){
-        rs422.put(5, -0.8, 0.0);
+    while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){
+        float cylinder = 0.0;
+        float sholderL = 0.0;
+        float sholderR = 0.0;
+        if(interrupt_cylinder_min == 1){
+            cylinder = 2.0;
+        }else{
+            cylinder = 0.0;
+        }
+        if(interrupt_sholderright_max == 1){
+            sholderR = -2.0;
+        }else{
+            sholderR = 0.0;
+        }
+        if(interrupt_sholderleft_max == 1){
+            sholderL = 2.0;
+        }else{
+            sholderL = 0.0;
+        }
+            rs422.put(5, cylinder, 0.0);
+            rs422.put(6, sholderR, sholderL);
     }
-    rs422.put(5, 0.0, 0.0);
     enc_cylinder.origin();
 }
 
@@ -275,30 +293,31 @@
     cylinder_reload.cyclic(sbdbt.down);
 
     static int counter;
+    static float mecanumV1, mecanumV2, mecanumV3, mecanumV4;
     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
 
     //sbdbtがpairingしている場合のみ動作
     if(sbdbt.get_pairingState()) {
         switch (counter) {
             case 0:
-                rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)));
+                rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)));
                 counter++;
                 break;
             case 1:
                 //.duty(<cal>*<powerControle>+(<boost>*0.5))
-                rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)));
+                rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)));
                 counter ++;
                 break;
             case 2:
-                rs422.put(id[counter], -1*enc_cylinder.duty_enableWidth(-5.0,5.0), 0.0);
+                rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-5.0,5.0)),1.0,-1.0), 0.0);
                 counter ++;
                 break;
             case 3:
-                rs422.put(id[counter], -1*shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power);
+                rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0));
                 counter ++;
                 break;
             case 4:
-                rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
+                rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0);
                 counter = 0;
                 break;
             default: