Diff: main.cpp

Revision:

38:b071512af5ca

Parent:

37:fa738e34c4d3

Child:

39:6735743ac0f1

--- a/main.cpp	Wed Sep 06 02:57:38 2017 +0000
+++ b/main.cpp	Thu Sep 07 02:21:24 2017 +0000
@@ -78,6 +78,7 @@
 DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
 Encoder enc_cylinder(encoder_A,encoder_B);
+Cyclic cyclic_servo;
 
 //追加
 PwmOut servo(pin_servo);
@@ -91,6 +92,7 @@
 void cylinder_cal();
 void boost();
 void cylinder_origin();
+void cylinder_origin_first();
 void sword_cal();
 void cylinder_reload();
 void servo_origin();
@@ -98,6 +100,7 @@
 float shoulder_left_cal();
 int reload_magazine_flag;
 float yaw, target_yaw;
+int cylinder_origin_flag = 0;
 
 //riseEventそのうちClassにしたい
 short state;
@@ -129,7 +132,7 @@
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
-    cylinder_origin();
+    cylinder_origin_first();
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad()+180.0);
@@ -172,7 +175,7 @@
 }
 
 //cylinder
-void cylinder_origin()
+void cylinder_origin_first()
 {
     while(interrupt_cylinder_min == 1) {
         led1 = 1;
@@ -181,6 +184,19 @@
     led1 = 0;
     enc_cylinder.origin();
 }
+
+//cylinder_origin_flagを1で動作する
+void cylinder_origin()
+{
+    if(cylinder_origin_flag == 1){
+        rs422.put(5, -0.2, 0.0);
+    }
+    if(interrupt_cylinder_min == 0){
+    cylinder_origin_flag = 0;
+    enc_cylinder.origin();
+    }
+}
+
 //追記(動作未確認)
 float cylinder_pwm;
 int cylinder_pos_num = 0;
@@ -197,7 +213,7 @@
     }
     enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定
 
-    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());
 
 //リロード機構完成後
     /*
@@ -214,24 +230,25 @@
         }
     */
 }
-//追記(動作未確認)
-//SBDBTか何かでcylinder_reload_enableを1にすりゃ動く
-int cylinder_reload_enable = 0;
-void cylinder_reload()
-{
-    if(cylinder_reload_enable == 1) {
-        cylinder_pos_num = 0;
-        if(interrupt_cylinder_min == 0) {
-            servo.pulsewidth(0.0022);
-            cylinder_reload_enable = 0;
-            timer_servo.attach(&servo_origin, servo_reload_time);
-        }
-    }
+
+void servo_max(){
+    servo.pulsewidth(0.0022);   
 }
-void servo_origin()
+
+void servo_min()
 {
     servo.pulsewidth(0.0010);
-    timer_servo.detach();
+}
+
+void servo_out(){
+    cyclic_servo.cyclic(sbdbt.down);
+    switch (cyclic_servo.getState()){
+        case 0:
+            servo_min();
+        case 1:
+            servo_max();
+        default:
+    }
 }
 
 
@@ -268,22 +285,19 @@
     motor_cal();
     cylinder_cal();
     sword_cal();
+    servo_out();
+    cylinder_origin();
     //boost();
     
     //インタラプタが反応したとき原点を取る
-    /*
-    if(interrupt_cylinder_min==0) {
-        enc_cylinder.origin();
+    if(sbdbt.up) {
+        cylinder_origin_flag = 1;
     }
-    */
     
     if(sbdbt.left) {
         cylinder_origin();
     }
     
-    if(sbdbt.down){
-        cylinder_reload_enable = 1;
-    }
 
     static int counter;
     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};