Diff: main.cpp

Revision:

42:63aedf71f4d1

Parent:

40:2d6888448ab2

Child:

43:605e5b0b9106

diff -r a4e37077833b -r 63aedf71f4d1 main.cpp
--- a/main.cpp	Fri Sep 08 04:00:07 2017 +0000
+++ b/main.cpp	Fri Sep 08 05:27:30 2017 +0000
@@ -51,13 +51,14 @@
 #define pin_interrupt_sholderleft_min   p8
 #define pin_servo p21
 #define servo_reload_time 1.0
+#define pin_cylinder_reload LED1
 //#define pin_servo_reload p29 //
 //#define pin_interrupt_reload p30 //
 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+//DigitalOut led1(LED1);
+//DigitalOut led2(LED2);
+//DigitalOut led3(LED3);
+//DigitalOut led4(LED4);
 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
@@ -79,6 +80,7 @@
 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
 Encoder enc_cylinder(encoder_A,encoder_B);
 Cyclic cyclic_servo;
+Cyclic_IO cylinder_reload(pin_cylinder_reload);
 
 //追加
 PwmOut servo(pin_servo);
@@ -94,10 +96,10 @@
 void cylinder_origin();
 void cylinder_origin_first();
 void sword_cal();
-void cylinder_reload();
 void servo_origin();
 float shoulder_right_cal();
 float shoulder_left_cal();
+
 //output
 float yaw, target_yaw;
 //cylinder_origin
@@ -105,7 +107,7 @@
 //cylinder
 float cylinder_pwm;
 int cylinder_pos_num = 0;
-float cylinder_pos[3] = {0.0,180.0,360.0};
+float cylinder_pos[3] = {0.0,90.0,325.0};
 
 //riseEventそのうちClassにしたい
 short state;
@@ -123,10 +125,10 @@
 {
     setup();
     while(1) {
-        led1 = interrupt_sholderleft_max;
-        led2 = interrupt_sholderleft_min;
-        led3 = interrupt_sholderright_max;
-        led4 = interrupt_sholderright_min;
+//        led1 = interrupt_sholderleft_max;
+//        led2 = interrupt_sholderleft_min;
+//        led3 = interrupt_sholderright_max;
+//        led4 = interrupt_sholderright_min;
         //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
         //pc.printf("rise state : %d\r\n",cyclic_servo.getState());
     }
@@ -142,6 +144,7 @@
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
     cylinder_origin_first();
+    cylinder_pos_num = 1;                   //セットアップタイムでの初期装填のため
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
@@ -192,7 +195,7 @@
     enc_cylinder.origin();
 }
 
-//cylinder_origin_flagを1で動作する
+//cylinder_origin_flagを1にすることで動作する
 void cylinder_origin()
 {
     if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1){
@@ -200,7 +203,7 @@
         enc_cylinder.origin();
         cylinder_pos_num = 0;
     }else if(cylinder_origin_flag == 1){
-        rs422.put(5, -0.5, 0.0);
+        rs422.put(5, -0.8, 0.0);
     }
 }
 
@@ -212,7 +215,8 @@
     if(riseEvent(sbdbt.right)) {        //cylinder degset
         cylinder_pos_num++;
         if(cylinder_pos_num >= 3) {
-            cylinder_pos_num = 0;
+            //cylinder_pos_num = 0;
+            cylinder_origin_flag=1;
         }
     }
     enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定
@@ -298,6 +302,8 @@
         cylinder_origin();
     }
     
+    cylinder_reload.cyclic(sbdbt.right);
+    
 
     static int counter;
     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};