Diff: main.cpp

Revision:

64:41dcec6c20bc

Parent:

59:68a73b9d27f5

Child:

65:5e4c2e5494ae

diff -r 9766aaa365db -r 41dcec6c20bc main.cpp
--- a/main.cpp	Thu Sep 21 05:59:49 2017 +0000
+++ b/main.cpp	Sat Sep 23 10:10:58 2017 +0000
@@ -39,9 +39,9 @@
 #define acceleration    15  //25
 #define pin_cylinder_on     p18
 #define pin_cylinder_off    p17
-#define pin_interrupt_cylinder_min       p23
-#define encoder_A       p25
-#define encoder_B       p26
+#define pin_interrupt_cylinder_min       p29
+#define encoder_A       p21
+#define encoder_B       p22
 #define enc_Kp      0.0400
 #define enc_Ki      0.0001
 #define enc_Kd      0.0003
@@ -52,7 +52,7 @@
 #define pin_interrupt_sholderright_min  p20 //p22
 #define pin_interrupt_sholderleft_max   p7
 #define pin_interrupt_sholderleft_min   p8
-#define pin_servo p21
+//#define pin_servo p21
 #define servo_reload_time 1.0
 #define pin_cylinder_reload p15
 #define pin_sbdbt_pairing p12
@@ -88,7 +88,7 @@
 Encoder enc_cylinder(encoder_A,encoder_B);
 Cyclic cyclic_servo;
 Cyclic_IO cylinder_reload(pin_cylinder_reload);
-PwmOut servo(pin_servo);
+//PwmOut servo(pin_servo);
 DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
 varEvent cylinder_event;
 
@@ -124,7 +124,7 @@
         led3 = interrupt_sholderright_max;
         led4 = interrupt_sholderright_min;
         
-        pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
+        //pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
         //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
     }
 }
@@ -132,9 +132,9 @@
 void setup()
 {
     wait(2.0);
-    bno055.begin();
+    //bno055.begin();
     wait(1.0);
-    bno055.firstRead();
+    //bno055.firstRead();
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
@@ -149,7 +149,7 @@
     v4.setup(acceleration,output_period);
     enc_cylinder.setup(100);
     enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
-    servo.period(0.020);
+    //servo.period(0.020);
 }
 
 //Sword
@@ -186,6 +186,10 @@
         float cylinder = 0.0;
         float sholderL = 0.0;
         float sholderR = 0.0;
+        led1 = interrupt_sholderleft_max;
+        led2 = interrupt_sholderleft_min;
+        led3 = interrupt_sholderright_max;
+        led4 = interrupt_sholderright_min;
         if(interrupt_cylinder_min == 1){
             cylinder = 2.0;
         }else{
@@ -239,10 +243,11 @@
     }
     enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //set cylinder_tergetPos
 
-    //pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());
+    pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());
 }
 
-void servo_max()
+//function
+/*void servo_max()
 {
     servo.pulsewidth(0.0022);
 }
@@ -258,10 +263,10 @@
     } else if(cyclic_servo.getState()==1) {
         servo_max();
     }
-}
+}*/
 
 
-//boost
+//functionBoost
 void boost()
 {
     if(sbdbt.r2) {
@@ -280,7 +285,7 @@
     */
 }
 
-//mecanum
+//functionMecanum
 void motor_cal()
 {
     static float out_right_x;
@@ -302,7 +307,7 @@
     motor_cal();
     cylinder_cal();
     sword_cal();
-    servo_out();
+    //servo_out();
     cylinder_origin();
     //boost();
     sbdbt_indigator = sbdbt.get_pairingState();