Diff: main.cpp

Revision:

45:a32e8091901b

Parent:

44:7410d8356f58

Child:

47:6ea046767494

diff -r 7410d8356f58 -r a32e8091901b main.cpp
--- a/main.cpp	Sun Sep 10 11:51:57 2017 +0000
+++ b/main.cpp	Tue Sep 12 08:29:39 2017 +0000
@@ -53,17 +53,19 @@
 #define pin_servo p21
 #define servo_reload_time 1.0
 #define pin_cylinder_reload p15
+#define pin_sbdbt_pairing p12
+#define pin_sbdbt_indicator p11
 //#define pin_servo_reload p29 //
 //#define pin_interrupt_reload p30 //
 
-//DigitalOut led1(LED1);
+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);
-Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
+Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing);
 Ticker output_timer;
 Mecanum mecanum;
 Bno055 bno055;
@@ -86,6 +88,7 @@
 //追加
 PwmOut servo(pin_servo);
 Timeout timer_servo;
+DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
 //Servo servo_reload(pin_servo_reload);
 //DigitalIn interrupt_reload(pin_interrupt_reload);
 
@@ -132,6 +135,7 @@
 //        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());
+        pc.printf("Sbdbtstate : %d\r\n",sbdbt.get_pairingState());
     }
 }
 
@@ -199,11 +203,11 @@
 //cylinder_origin_flagを1にすることで動作する
 void cylinder_origin()
 {
-    if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1){
+    if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
         cylinder_origin_flag = 0;
         enc_cylinder.origin();
         cylinder_pos_num = 0;
-    }else if(cylinder_origin_flag == 1){
+    } else if(cylinder_origin_flag == 1) {
         rs422.put(5, -0.8, 0.0);
     }
 }
@@ -240,18 +244,20 @@
     */
 }
 
-void servo_max(){
-    servo.pulsewidth(0.0022);   
+void servo_max()
+{
+    servo.pulsewidth(0.0022);
 }
 void servo_min()
 {
     servo.pulsewidth(0.0010);
 }
-void servo_out(){
+void servo_out()
+{
     cyclic_servo.cyclic(sbdbt.down);    //setServoControl
-    if(cyclic_servo.getState()==0){
+    if(cyclic_servo.getState()==0) {
         servo_min();
-    }else if(cyclic_servo.getState()==1){
+    } else if(cyclic_servo.getState()==1) {
         servo_max();
     }
 }
@@ -293,39 +299,49 @@
     servo_out();
     cylinder_origin();
     //boost();
-    
-    //インタラプタが反応したとき原点を取る
+
+    led1 = sbdbt.get_pairingState();
+    sbdbt_indigator = sbdbt.get_pairingState();
+
     if(sbdbt.up) {
         cylinder_origin_flag = 1;
     }
-    
+
     cylinder_reload.cyclic(sbdbt.left);
-    
+
     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)+(sbdbt.right_y*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5)));
-            counter++;
-            break;
-        case 1:
-            //.duty(<cal>*<powerControle>+(<boost>*0.5))
-            rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.5)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.5)));
-            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_left_cal()*sholder_power);
-            counter ++;
-            break;
-        case 4:
-            rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
-            counter = 0;
-            break;
-        default:
-            break;
-    };
+
+    //sbdbtがpairingしている場合のみ動作
+    if(sbdbt.get_pairingState()) {
+        switch (counter) {
+            case 0:
+                rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5)));
+                counter++;
+                break;
+            case 1:
+                //.duty(<cal>*<powerControle>+(<boost>*0.5))
+                rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.5)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.5)));
+                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_left_cal()*sholder_power);
+                counter ++;
+                break;
+            case 4:
+                rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
+                counter = 0;
+                break;
+            default:
+                break;
+        };
+    }else{
+        for(int count_i = 0;count_i<=nucleo_num;count_i++){
+            rs422.put(id[count_i],0.0,0.0);
+        }
+    }
 }
\ No newline at end of file