Diff: main.cpp

Revision:

55:2dd2f161ebaf

Parent:

54:a88208d9bc1d

Child:

56:a7bd860b85b6

--- a/main.cpp	Sat Sep 16 01:05:23 2017 +0000
+++ b/main.cpp	Sat Sep 16 05:59:00 2017 +0000
@@ -55,6 +55,7 @@
 #define pin_cylinder_reload p15
 #define pin_sbdbt_pairing p12
 #define pin_sbdbt_indicator p11
+#define cylinder_pos_max 4
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -84,7 +85,6 @@
 Cyclic_IO cylinder_reload(pin_cylinder_reload);
 PwmOut servo(pin_servo);
 DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
-varEvent event;
 varEvent cylinder_event;
 
 void setup();
@@ -106,7 +106,7 @@
 //cylinder
 float cylinder_pwm;
 int cylinder_pos_num = 0;
-float cylinder_pos[3] = {0.0,90.0,325.0};
+float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,700.0};
 
 int main()
 {
@@ -117,8 +117,8 @@
         led3 = interrupt_sholderright_max;
         led4 = interrupt_sholderright_min;
         
-        event.input(sbdbt.left);
-        pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
+        pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
+        //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
     }
 }
 
@@ -211,7 +211,6 @@
         rs422.put(5, -0.8, 0.0);
     }
 }
-
 void cylinder_cal()
 {
     cylinder.cyclic(sbdbt.shikaku);     //cylinder ON/OFF
@@ -219,7 +218,7 @@
     cylinder_event.input(sbdbt.right);
     if(cylinder_event.getRise()) {        //cylinder degset
         cylinder_pos_num++;
-        if(cylinder_pos_num >= 3) {
+        if(cylinder_pos_num >= cylinder_pos_max) {
             //cylinder_pos_num = 0;
             cylinder_origin_flag=1;
         }
@@ -273,7 +272,7 @@
     yaw = bno055.getYawRad();
     target_yaw = yaw;
     yaw_pid.cal(target_yaw, yaw, output_period);
-    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
+    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, sbdbt.right_x, 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()
@@ -309,6 +308,7 @@
                 counter ++;
                 break;
             case 2:
+                //rs422.put(id[counter], sbdbt.right_y, 0.0);
                 rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-5.0,5.0)),1.0,-1.0), 0.0);
                 counter ++;
                 break;