Diff: main.cpp

Revision:

35:5e1ad00f26fb

Parent:

34:02d605c68bf3

Child:

36:dca1081c19b3

diff -r 02d605c68bf3 -r 5e1ad00f26fb main.cpp
--- a/main.cpp	Tue Aug 29 11:30:28 2017 +0000
+++ b/main.cpp	Wed Sep 06 01:17:44 2017 +0000
@@ -13,8 +13,6 @@
 #include "cyclic.h"
 #include "cyclic_IO.h"
 #include "cylinder.h"
-#include "Servo.h"
-//#include "QEI.h"
 
 #define pc_baud     460800
 #define sbdbt_tx    p13
@@ -47,12 +45,14 @@
 #define mecanum_power   1.0
 #define sword_power     0.8
 #define sholder_power   0.8
-//#define pin_servo_reload p29 //
-//#define pin_interrupt_reload p30 //
 #define pin_interrupt_sholderright_max  p21
 #define pin_interrupt_sholderright_min  p22
 #define pin_interrupt_sholderleft_max   p7
 #define pin_interrupt_sholderleft_min   p8
+#define pin_servo p29
+#define servo_reload_time 1.0
+//#define pin_servo_reload p29 //
+//#define pin_interrupt_reload p30 //
 
 DigitalOut led1(LED1);
 //DigitalOut led2(LED2);
@@ -77,11 +77,11 @@
 DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
 DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
-
+Encoder enc_cylinder(encoder_A,encoder_B);
 
-//追加点
-Encoder enc_cylinder(encoder_A,encoder_B);
-//QEI wheel(encoder_A, encoder_B, NC, 624);
+//追加
+PwmOut servo(pin_servo);
+Timeout timer_servo;
 //Servo servo_reload(pin_servo_reload);
 //DigitalIn interrupt_reload(pin_interrupt_reload);
 
@@ -92,6 +92,8 @@
 void boost();
 void cylinder_origin();
 void sword_cal();
+void cylinder_reload();
+void servo_origin();
 float shoulder_right_cal();
 float shoulder_left_cal();
 int reload_magazine_flag;
@@ -120,9 +122,9 @@
 
 void setup()
 {
-    wait(1);
+    wait(0.5);
     bno055.begin();
-    wait(1);
+    wait(0.5);
     bno055.firstRead();
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
@@ -135,73 +137,34 @@
     v2.setup(acceleration,output_period);
     v3.setup(acceleration,output_period);
     v4.setup(acceleration,output_period);
-
-//追加点
     enc_cylinder.setup(100);
     enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
+
+    //追加
+    servo.period(0.020);
 }
 
-void output()
+
+float shoulder_right_cal()
 {
-    motor_cal();
-    cylinder_cal();
-    sword_cal();
-    //boost();
-    
-    if(interrupt_cylinder_min==0){
-        enc_cylinder.origin();
-    }
-    if(sbdbt.left){
-        cylinder_origin();
-    }
-
-    static int counter;
-    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
-
-    switch (counter) {
-        case 0:
-            rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power));
-            counter++;
-            break;
-        case 1:
-            rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power));
-            counter ++;
-            break;
-        case 2:
-            rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0);
-            counter ++;
-            break;
-        case 3:
-            rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_right_cal()*sholder_power);
-            counter ++;
-            break;
-        case 4:
-            rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
-            counter = 0;
-            break;
-        default:
-            break;
-    };
-}
-
-float shoulder_right_cal(){
-    if(interrupt_sholderright_max==0&&sbdbt.sankaku){
+    if(interrupt_sholderright_max==0&&sbdbt.sankaku) {
         return 0.0;
     }
-    if(interrupt_sholderright_min==0&&sbdbt.batu){
+    if(interrupt_sholderright_min==0&&sbdbt.batu) {
         return 0.0;
     }
     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
 }
 
-float shoulder_left_cal(){
-    if(interrupt_sholderleft_max==0&&sbdbt.up==1){
+float shoulder_left_cal()
+{
+    if(interrupt_sholderleft_max==0&&sbdbt.up==1) {
         return 0.0;
     }
-    if(interrupt_sholderleft_min==0&&sbdbt.down==1){
-        return 0.0;   
+    if(interrupt_sholderleft_min==0&&sbdbt.down==1) {
+        return 0.0;
     }
-    return (sbdbt.up*0.8-sbdbt.down*0.8);   
+    return (sbdbt.up*0.8-sbdbt.down*0.8);
 }
 
 void sword_cal()
@@ -219,6 +182,7 @@
     enc_cylinder.origin();
 }
 
+
 //追記(動作未確認)
 float cylinder_pwm;
 int cylinder_pos_num = 0;
@@ -226,15 +190,15 @@
 void cylinder_cal()
 {
     cylinder.cyclic(sbdbt.shikaku);     //cylinder ON/OFF
-    
-    if(riseEvent(sbdbt.right)){         //cylinder degset
+
+    if(riseEvent(sbdbt.right)) {        //cylinder degset
         cylinder_pos_num++;
-        if(cylinder_pos_num >= 3){
+        if(cylinder_pos_num >= 3) {
             cylinder_pos_num = 0;
         }
     }
     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());
 
 //リロード機構完成後
@@ -253,6 +217,27 @@
     */
 }
 
+//追記(動作未確認)
+//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_origin()
+{
+    servo.pulsewidth(0.0010);
+    timer_servo.detach();
+}
+
 void boost()
 {
     if(sbdbt.r2) {
@@ -278,4 +263,55 @@
     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()
+{
+    motor_cal();
+    cylinder_cal();
+    sword_cal();
+    //boost();
+    
+    //インタラプタが反応したとき原点を取る
+    /*
+    if(interrupt_cylinder_min==0) {
+        enc_cylinder.origin();
+    }
+    */
+    
+    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};
+    switch (counter) {
+        case 0:
+            rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power));
+            counter++;
+            break;
+        case 1:
+            rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power));
+            counter ++;
+            break;
+        case 2:
+            rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0);
+            counter ++;
+            break;
+        case 3:
+            rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_right_cal()*sholder_power);
+            counter ++;
+            break;
+        case 4:
+            rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
+            counter = 0;
+            break;
+        default:
+            break;
+    };
 }
\ No newline at end of file