sampleProgram

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_fuzi by kusano kiyoshige

Revision:
14:aac2f18f6779
Parent:
13:dfae731e239f
Child:
15:0fdf483769bf
--- a/main.cpp	Mon Jul 17 04:55:49 2017 +0000
+++ b/main.cpp	Fri Aug 04 00:16:44 2017 +0000
@@ -10,7 +10,8 @@
 #include "limit.h"
 #include "accelerator.h"
 #include "encorder.h"
-#include "cyclick.h"
+#include "cyclic.h"
+#include "cyclic_IO.h"
 //#include "QEI.h"
 
 #define pc_baud     460800
@@ -26,6 +27,8 @@
 #define n1_id       3
 #define n2_id       4
 #define n3_id       5
+#define n4_id       6
+#define n5_id       7
 #define yaw_Kp      0.01
 #define yaw_Ki      0.01
 #define yaw_Kd      0.01
@@ -55,6 +58,7 @@
 Cyclic_IO led2(LED2);
 Cyclic_IO sylinder_on(pin_cylinder_on);
 Cyclic_IO sylinder_off(pin_cylinder_off);
+Cyclic cylinder_cyclic;
 
 //追加点
 Encoder enc_cylinder(encoder_A,encoder_B);
@@ -72,7 +76,7 @@
 {
     setup();
     while(1) {
-        led1 = interrupt;
+        //led1 = interrupt;
         //pc.printf("Pulses is: %i\r\n",wheel.getPulses());
     }
 }
@@ -86,6 +90,7 @@
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
+    cylinder_origin();
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad()+180.0);
@@ -93,7 +98,6 @@
     v2.setup(acceleration,output_period);
     v3.setup(acceleration,output_period);
     v4.setup(acceleration,output_period);
-    //cylinder_origin();
 
 //追加点
     enc_cylinder.setup(1200);
@@ -102,8 +106,10 @@
 
 void cylinder_origin(){
     while(interrupt){
+        led1 = 1;
         rs422.put(5, -0.8, 0.0);
     }
+    led1 = 0;
 }
 
 void boost(){
@@ -132,6 +138,13 @@
     }
     led2.cyclic(sbdbt.left);
     */
+    
+    cylinder_cyclic.cyclic(sbdbt.up);
+    if(cylinder_cyclic.getState()== 1){
+        led1 = 1;
+    }else{
+        led1 = 0;
+    }
 }
 
 void put_output()
@@ -168,5 +181,4 @@
         default:
             break;
     };
-}
-
+}
\ No newline at end of file