Diff: main.cpp

Revision:

36:dca1081c19b3

Parent:

35:5e1ad00f26fb

Child:

37:fa738e34c4d3

--- a/main.cpp	Wed Sep 06 01:17:44 2017 +0000
+++ b/main.cpp	Wed Sep 06 02:22:16 2017 +0000
@@ -144,7 +144,7 @@
     servo.period(0.020);
 }
 
-
+//Sword
 float shoulder_right_cal()
 {
     if(interrupt_sholderright_max==0&&sbdbt.sankaku) {
@@ -164,14 +164,14 @@
     if(interrupt_sholderleft_min==0&&sbdbt.down==1) {
         return 0.0;
     }
-    return (sbdbt.up*0.8-sbdbt.down*0.8);
+    return (sbdbt.sankaku*0.8-sbdbt.batu*0.8);
 }
-
 void sword_cal()
 {
     sword.cyclic(sbdbt.maru);
 }
 
+//cylinder
 void cylinder_origin()
 {
     while(interrupt_cylinder_min == 1) {
@@ -181,8 +181,6 @@
     led1 = 0;
     enc_cylinder.origin();
 }
-
-
 //追記(動作未確認)
 float cylinder_pwm;
 int cylinder_pos_num = 0;
@@ -216,7 +214,6 @@
         }
     */
 }
-
 //追記(動作未確認)
 //SBDBTか何かでcylinder_reload_enableを1にすりゃ動く
 int cylinder_reload_enable = 0;
@@ -231,13 +228,14 @@
         }
     }
 }
-
 void servo_origin()
 {
     servo.pulsewidth(0.0010);
     timer_servo.detach();
 }
 
+
+//boost
 void boost()
 {
     if(sbdbt.r2) {
@@ -256,6 +254,7 @@
     */
 }
 
+//mecanum
 void motor_cal()
 {
     yaw = bno055.getYawRad();
@@ -264,8 +263,6 @@
     mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
 //    pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY());
 }
-
-
 void output()
 {
     motor_cal();
@@ -304,7 +301,7 @@
             counter ++;
             break;
         case 3:
-            rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_right_cal()*sholder_power);
+            rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power);
             counter ++;
             break;
         case 4: