Diff: main.cpp

Revision:

15:0fdf483769bf

Parent:

14:aac2f18f6779

Child:

16:e49df474e4c6

--- a/main.cpp	Fri Aug 04 00:16:44 2017 +0000
+++ b/main.cpp	Fri Aug 04 01:35:16 2017 +0000
@@ -12,6 +12,7 @@
 #include "encorder.h"
 #include "cyclic.h"
 #include "cyclic_IO.h"
+#include "cylinder.h"
 //#include "QEI.h"
 
 #define pc_baud     460800
@@ -43,6 +44,9 @@
 #define enc_Kd      0.01
 
 DigitalOut led1(LED1);
+//DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+//DigitalOut led4(LED4);
 DigitalIn interrupt(pin_interrupt);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
@@ -55,10 +59,7 @@
 Accel v2;
 Accel v3;
 Accel v4;
-Cyclic_IO led2(LED2);
-Cyclic_IO sylinder_on(pin_cylinder_on);
-Cyclic_IO sylinder_off(pin_cylinder_off);
-Cyclic cylinder_cyclic;
+Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
 
 //追加点
 Encoder enc_cylinder(encoder_A,encoder_B);
@@ -66,8 +67,8 @@
 
 void setup();
 void output();
-void put_output();
-void cylinder_check();
+void motor_cal();
+void cylinder();
 void boost();
 void cylinder_origin();
 float yaw, target_yaw;
@@ -76,21 +77,20 @@
 {
     setup();
     while(1) {
-        //led1 = interrupt;
-        //pc.printf("Pulses is: %i\r\n",wheel.getPulses());
+        pc.printf("Pulses is: %i\r\n",sbdbt.down);
     }
 }
 
 void setup()
 {
     wait(1);
-    bno055.begin();
+//    bno055.begin();
     wait(1);
-    bno055.firstRead();
+//    bno055.firstRead();
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
-    cylinder_origin();
+//    cylinder_origin();
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad()+180.0);
@@ -104,6 +104,34 @@
     enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
 }
 
+void output()
+{
+    motor_cal();
+    cylinder();
+    boost();
+
+    static int counter;
+    int id[nucleo_num] = {n1_id, n2_id, n3_id};
+
+    switch (counter) {
+        case 0:
+            rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
+            counter++;
+            break;
+        case 1:
+            rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
+            counter ++;
+            break;
+        case 2:
+            rs422.put(id[counter], sbdbt.right_y, 0.0);
+            counter = 0;
+            break;
+        default:
+            break;
+    };
+}
+
+
 void cylinder_origin(){
     while(interrupt){
         led1 = 1;
@@ -127,58 +155,16 @@
     }
 }
 
-void cylinder_check()
+void cylinder()
 {
-    //printf("up\t%d\tstate\t%d\tdown\t%d\tstate\t%d\t\r\n",sbdbt.up,sylinder_on.getState(),sbdbt.down,sylinder_off.getState());
-    /*
-    if(sylinder_on?){
-    sylinder_on.cyclic(sbdbt.up);
-    }else if(){
-    sylinder_off.cyclic(sbdbt.down); 
-    }
-    led2.cyclic(sbdbt.left);
-    */
-    
-    cylinder_cyclic.cyclic(sbdbt.up);
-    if(cylinder_cyclic.getState()== 1){
-        led1 = 1;
-    }else{
-        led1 = 0;
-    }
+    cylinder.cyclic(sbdbt.down);
 }
 
-void put_output()
+void motor_cal()
 {
     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());
 //    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()
-{
-    put_output();
-    cylinder_check();
-    boost();
-
-    static int counter;
-    int id[nucleo_num] = {n1_id, n2_id, n3_id};
-
-    switch (counter) {
-        case 0:
-            rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
-            counter++;
-            break;
-        case 1:
-            rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
-            counter ++;
-            break;
-        case 2:
-            rs422.put(id[counter], sbdbt.right_y, 0.0);
-            counter = 0;
-            break;
-        default:
-            break;
-    };
 }
\ No newline at end of file