Diff: main.cpp

Revision:

31:285c9898da03

Parent:

30:57061e222f10

Child:

32:f535ace7c529

--- a/main.cpp	Thu Aug 24 18:20:12 2017 +0000
+++ b/main.cpp	Tue Aug 29 02:07:56 2017 +0000
@@ -38,7 +38,7 @@
 #define acceleration    25
 #define pin_cylinder_on     p17
 #define pin_cylinder_off    p18
-#define pin_interrupt       p23
+#define pin_interrupt_cylinder_min       p23
 #define encoder_A       p25
 #define encoder_B       p26
 #define enc_Kp      0.01
@@ -47,12 +47,16 @@
 #define powerdown   0.6
 #define pin_servo_reload p21 //
 #define pin_interrupt_reload p22 //
+#define pin_interrupt_sholderright_max  p23
+#define pin_interrupt_sholderright_min  p24
+#define pin_interrupt_sholderleft_max   p29
+#define pin_interrupt_sholderleft_min   p30
 
 DigitalOut led1(LED1);
 //DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 //DigitalOut led4(LED4);
-DigitalIn interrupt(pin_interrupt);
+DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
@@ -66,6 +70,13 @@
 Accel v4;
 Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
 Cyclic sword;
+Cyclic cyclic_cylinder_position;
+DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
+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);
 //QEI wheel(encoder_A, encoder_B, NC, 624);
@@ -79,14 +90,29 @@
 void boost();
 void cylinder_origin();
 void sword_cal();
+float shoulder_right_cal();
+float shoulder_left_cal();
 int reload_magazine_flag;
 float yaw, target_yaw;
 
+//riseEventそのうちClassにしたい
+short state;
+int riseEvent(int input)
+{
+    state = ((state<<1)|input)&3;
+    if(state == 1) {
+        return 1;
+    } else {
+        return 0;
+    }
+}
+
 int main()
 {
     setup();
     while(1) {
-        pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
+        //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
+        //pc.printf("rise state : %d\r\n",riseEvent(sbdbt.right));
     }
 }
 
@@ -109,7 +135,7 @@
     v4.setup(acceleration,output_period);
 
 //追加点
-    enc_cylinder.setup(1200);
+    enc_cylinder.setup(100);
     enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
 }
 
@@ -137,7 +163,7 @@
             counter ++;
             break;
         case 3:
-            rs422.put(id[counter], (-sbdbt.sankaku*0.8+sbdbt.batu*0.8), (sbdbt.up*0.8-sbdbt.down*0.8));
+            rs422.put(id[counter], shoulder_right_cal(),shoulder_right_cal());
             counter ++;
             break;
         case 4:
@@ -149,42 +175,81 @@
     };
 }
 
-void sword_cal(){
+float shoulder_right_cal(){
+    if(interrupt_sholderright_max==1&&sbdbt.batu==1){
+        return 0.0;
+    }
+    if(interrupt_sholderright_min==1&&sbdbt.sankaku==1){
+        return 0.0;   
+    }
+    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
+}
+
+float shoulder_left_cal(){
+    if(interrupt_sholderright_max==1&&sbdbt.up==1){
+        return 0.0;
+    }
+    if(interrupt_sholderright_min==1&&sbdbt.down==1){
+        return 0.0;   
+    }
+    return (sbdbt.up*0.8-sbdbt.down*0.8);   
+}
+
+void sword_cal()
+{
     sword.cyclic(sbdbt.maru);
 }
 
-void cylinder_origin(){
-    while(interrupt){
+void cylinder_origin()
+{
+    while(interrupt_cylinder_min == 1) {
         led1 = 1;
-        rs422.put(5, -0.8, 0.0);
+        rs422.put(5, -0.2, 0.0);
     }
     led1 = 0;
+    enc_cylinder.origin();
 }
 
 //追記(動作未確認)
+float cylinder_pwm;
+int cylinder_pos_num = 0;
+float cylinder_pos[3] = {0.0,50.0,75.0};
 void cylinder_cal()
-{   
-    cylinder.cyclic(sbdbt.shikaku);
-    enc_cylinder.deg_cylinder_cal();
+{
+    cylinder.cyclic(sbdbt.shikaku);     //cylinder ON/OFF
+    
+    if(riseEvent(sbdbt.right)){         //cylinder degset
+        cylinder_pos_num++;
+        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_pulse\t%lo\tnow_rad\t%f\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.pulse(),enc_cylinder.deg(),enc_cylinder.duty());
 
-    if(cylinder.getInState() == 1){
-        if(interrupt)servo_reload = 1.0;
-    }else{
-        servo_reload = 0.0;
-        reload_magazine_flag = 1;
-    }
-    
-    if(reload_magazine_flag == 1){
-        rs422.put(n6_id, 0.8, 0.0);
-        if(interrupt_reload == 1)reload_magazine_flag = 0;
-    }
+//リロード機構完成後
+    /*
+        if(cylinder.getInState() == 1){
+            if(interrupt_cylinder_min)servo_reload = 1.0;
+        }else{
+            servo_reload = 0.0;
+            reload_magazine_flag = 1;
+        }
+
+        if(reload_magazine_flag == 1){
+            rs422.put(n6_id, 0.8, 0.0);
+            if(interrupt_reload == 1)reload_magazine_flag = 0;
+        }
+    */
 }
 
-void boost(){
-    if(sbdbt.r2){
+void boost()
+{
+    if(sbdbt.r2) {
         mecanum.boost_forward();
     }
-    if(sbdbt.l2){
+    if(sbdbt.l2) {
         mecanum.boost_back();
     }
     /*