Diff: main.cpp

Revision:

22:37a07f844778

Parent:

21:90bb12a879c2

Child:

23:ffe4f63bee36

diff -r 90bb12a879c2 -r 37a07f844778 main.cpp
--- a/main.cpp	Thu Mar 29 02:23:22 2018 +0000
+++ b/main.cpp	Thu Apr 05 07:52:54 2018 +0000
@@ -5,8 +5,6 @@
 #define R_C_MAX 2
  
 Serial pc(USBTX,USBRX);
-I2C i2c(p9, p10);       // sda, scl
-I2C i2c_2(p28, p27);
  
 DigitalOut grab(p11);   //0:開く 1:閉じる
 DigitalOut snap(p14);   //1:装填 0:発射
@@ -16,13 +14,14 @@
 DigitalOut snap2(p15);   //1:装填 0:発射
 DigitalOut s_grab2(p16); //0:解放 1:把持
 DigitalOut ledw(p20);   //射出LED
- 
+DigitalIn enable(p21);
+
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 DigitalOut LEDN(p10);
-DigitalOut out(p21);
+//DigitalOut out(p21);
  
 std_msgs::String action;
 std_msgs::String fin_or_not;
@@ -32,17 +31,8 @@
 int have_cock=0;    //コック持ってますか？
 int have_action=0;  //装填実行
 int ready_action=0; //装填待機
-int r,g,b,a;
-int val;
-int red=0;
-int val2;
-int red2=0;
-int RED_MIN=0;
-int RED_MIN2=0;
 int32_t act=101;
 int ball_judgement=1;
-int r_c=0;
-int r_c2=0;
 int finding=0;
  
 Ticker MC;
@@ -80,8 +70,6 @@
 void MCL()
 {
     pub_b.data=ball_judgement;
-    pub_c1.data=RED_MIN2;
-    pub_c2.data=r_c2;
     pub_action.publish(&pub_act);
     pub_color1.publish(&pub_c1);
     pub_color2.publish(&pub_c2);
@@ -196,7 +184,7 @@
 {
     snap=1;
     snap2=1;
-    out=1;
+    //out=1;
     
     nh.initNode();
     nh.subscribe(sub);
@@ -211,54 +199,32 @@
     pub_act.data=0;
     pub_fin.data=0;
     pub_b.data=0;
-    char cmd[2];
-    char cell[1]= {0x03};
-    char data[8]= {0,0,0,0,0,0,0,0};
-    char data2[8]= {0,0,0,0,0,0,0,0};
-    cmd[0] = 0x00;
-    cmd[1] = 0x89;
-    i2c.frequency(115200);
-    i2c_2.frequency(115200);
-    val = i2c.write(84, cmd, 2);
-    val2 = i2c_2.write(84, cmd, 2);
-    cmd[0] = 0x0;
-    cmd[1] = 0x09;
-    val = i2c.write(84, cmd, 2);
-    val2 = i2c_2.write(84, cmd, 2);
-    
+
     while(1) {
-        val = i2c.write(84, cell, 1);
-        val2 = i2c_2.write(84, cell, 1);
-        val = i2c.read(84, data, 8);
-        val2 = i2c_2.read(84, data2, 8);
-        red = data[0]<<8 | data[1];
-        red2 = data2[0]<<8 | data2[1];
-       /* if(red<RED_MIN) {
-            led1=1;
-            led2=0;
-        } else {
-            led1=0;
-            led2=1;
-        }*/
-        
-        //pc.printf("%d",red); ///カラーセンサー閾値
-        
+    if(enable){
+        count=0;        //何回actionしたんですか?
+        have_cock=0;    //コック持ってますか？
+        have_action=0;  //装填実行
+        ready_action=0; //装填待機
+        act=101;
+        ball_judgement=1;
+        finding=0;
+        snap=1;
+        snap2=1;
+        //out=1;
+        led1=1;
+        led2=1;
+        led3=1;
+        led4=1;
+    }    
+     else{
+         led1=0;
+         led2=0;
+         led3=0;
+         led4=0;
+         }   
     ///装填///
         if(((act==1)||(act==3)||(act==5)||(act==10)||(act==11))&&(ready_action==0)) { //TZ3
-                 /*if(act==1) {
-                    led1=1;
-                }
-                else if(act==3) {
-                    led1=1;
-                    led2=1;
-                }
-                else if(act==5) {
-                    led1=1;
-                    led3=1;
-                }
-                else if(act==8) {
-                    led4=1;
-                }*/
                 snap=1;
                 snap2=1;
                 grab=0;
@@ -337,13 +303,13 @@
                 count=0;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
-                r_c=0;
             }
         
     ///射出///
         if((act==2)&&(count==0)) {
             int f_i=0;
-            throw_cock(0.247); //TZ1
+            //throw_cock(0.247); //TZ1
+            throw_cock(0.242); //TZ1 03_29 0.247-down
  
             ready_action=0;
             count=1;
@@ -358,8 +324,9 @@
  
          if(((act==4)||(act==6))&&(count==0)) {
              int f_i=0;
-            throw_cock(0.257); //TZ2
- 
+            //throw_cock(0.257); //TZ2
+            throw_cock(0.238); //TZ2 03_29 0.257-tyou down
+            
             ready_action=0;
             count=1;
             pub_act.data=act;
@@ -373,7 +340,8 @@
         
          if(((act==7)||(act==8)||(act==9))&&(count==0))  {
             int f_i=0;
-            throw_cock(0.227); //TZ3
+            //throw_cock(0.227); //TZ3
+            throw_cock(0.221); //TZ3 0.227-down
  
             ready_action=0;
             count=1;