Diff: main.cpp

Revision:

28:72b9af7e1700

Parent:

27:2ffbc5587399

Child:

29:f380201ca30f

--- a/main.cpp	Thu Aug 24 16:40:04 2017 +0000
+++ b/main.cpp	Thu Aug 24 17:50:01 2017 +0000
@@ -13,6 +13,7 @@
 #include "cyclic.h"
 #include "cyclic_IO.h"
 #include "cylinder.h"
+#include "Servo.h"
 //#include "QEI.h"
 
 #define pc_baud     460800
@@ -23,13 +24,14 @@
 #define rs422_rx    p27
 #define rs422_baud  115200
 #define output_period   0.015
-#define nucleo_num  5
+#define nucleo_num  6
 #define pi          3.141592
 #define n1_id       3
 #define n2_id       4
 #define n3_id       5
 #define n4_id       6
 #define n5_id       7
+#define n6_id       8
 #define yaw_Kp      0.01
 #define yaw_Ki      0.01
 #define yaw_Kd      0.01
@@ -43,6 +45,8 @@
 #define enc_Ki      0.01
 #define enc_Kd      0.01
 #define powerdown   0.6
+#define pin_servo_reload p21 //
+#define pin_interrupt_reload p22 //
 
 DigitalOut led1(LED1);
 //DigitalOut led2(LED2);
@@ -62,10 +66,11 @@
 Accel v4;
 Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
 Cyclic sword;
-
 //追加点
 Encoder enc_cylinder(encoder_A,encoder_B);
 //QEI wheel(encoder_A, encoder_B, NC, 624);
+Servo servo_reload(pin_servo_reload);
+DigitalIn interrupt_reload(pin_interrupt_reload);
 
 void setup();
 void output();
@@ -116,7 +121,7 @@
     //boost();
 
     static int counter;
-    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id};
+    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
 
     switch (counter) {
         case 0:
@@ -156,10 +161,22 @@
     led1 = 0;
 }
 
+//追記(動作未確認)
 void cylinder_cal()
 {
     cylinder.cyclic(sbdbt.shikaku);
     enc_cylinder.deg_cylinder_cal();
+
+    if(cylinder.getInState()){
+        if(interrupt_reload == 0){
+            rs422.put(n6_id, 0.8, 0.0);
+        }else{
+            servo_reload =+ 0.05;
+            if(servo_reload >= 1.0)servo_reload = 1.0;
+        }
+    }else{
+        servo_reload = 0.0;
+    }
 }
 
 void boost(){