Diff: main.cpp

Revision:

17:5c4718c603dc

Parent:

16:e49df474e4c6

Child:

18:ae4d97945b83

diff -r e49df474e4c6 -r 5c4718c603dc main.cpp
--- a/main.cpp	Fri Aug 04 01:45:44 2017 +0000
+++ b/main.cpp	Fri Aug 04 02:14:02 2017 +0000
@@ -23,7 +23,7 @@
 #define rs422_rx    p27
 #define rs422_baud  115200
 #define output_period   0.015
-#define nucleo_num  3
+#define nucleo_num  4
 #define pi          3.141592
 #define n1_id       3
 #define n2_id       4
@@ -60,6 +60,7 @@
 Accel v3;
 Accel v4;
 Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
+Cyclic sword_left;
 
 //追加点
 Encoder enc_cylinder(encoder_A,encoder_B);
@@ -71,13 +72,14 @@
 void cylinder_cal();
 void boost();
 void cylinder_origin();
+void sword_left_cal();
 float yaw, target_yaw;
 
 int main()
 {
     setup();
     while(1) {
-        pc.printf("Pulses is: %i\r\n",sbdbt.down);
+        //pc.printf("Pulses is: %i\r\n",sbdbt.down);
     }
 }
 
@@ -108,10 +110,11 @@
 {
     motor_cal();
     cylinder_cal();
+    sword_left_cal();
     boost();
 
     static int counter;
-    int id[nucleo_num] = {n1_id, n2_id, n3_id};
+    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id};
 
     switch (counter) {
         case 0:
@@ -123,7 +126,11 @@
             counter ++;
             break;
         case 2:
-            rs422.put(id[counter], sbdbt.right_y, 0.0);
+            rs422.put(id[counter], 0.0, 0.0);
+            counter ++;
+            break;
+        case 3:
+            rs422.put(id[counter], sbdbt.right_y, ((float)sword_left.getState()));
             counter = 0;
             break;
         default:
@@ -131,6 +138,9 @@
     };
 }
 
+void sword_left_cal(){
+    sword_left.cyclic(sbdbt.maru);
+}
 
 void cylinder_origin(){
     while(interrupt){