Diff: main.cpp

Revision:

6:ca4fab2957e4

Parent:

5:4f1f0294d6aa

Child:

7:2827264ddf53

--- a/main.cpp	Thu Sep 21 15:24:44 2017 +0000
+++ b/main.cpp	Sat Nov 25 06:04:55 2017 +0000
@@ -1,68 +1,192 @@
-#include <mbed.h>
+#include <mbed.h>///////////////////////レーザーとカラーセンサーなしのwait受け渡し用
 #include <ros.h>
+#include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
-
-DigitalOut grab(p11); //0:閉じる 1:開く
-DigitalOut snap(p12); //0:縮む 1:伸びる
-std_msgs::String action;
-int k_action=0;
-int o_action=0;
-int m_action=0;
-void messageCallback(const std_msgs::String &msg)
-{
-    action.data=msg.data;
-}
-
-ros::NodeHandle nh;
-ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
+#define RED_MIN 3000
+ 
+//s_grab: 1:閉じる,0:開く
+//grab: 1close,2:open
+//snap:1 slow 0:catch
+//s_grab 1:close 0:open
+Serial pc(USBTX,USBRX);
+I2C i2c(p9, p10);        // sda, scl
+ 
+DigitalOut grab(p11); //0:開く 1:閉じる
+DigitalOut snap(p14); //1:装填 0:発射
+DigitalOut s_grab(p13); //0:otiru 1:ageru
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-
-void led_on(int l1,int l2,int l3,int l4)
+PwmOut servo(p21);
+std_msgs::String action;
+std_msgs::String fin_or_not;
+std_msgs::String get_or_not;
+int count=0;
+//何回actionしたんですか?
+int have_cock=0;
+//コック持ってますか？
+int have_action=0;
+//装填実行
+int ready_action=0; //装填待機
+int r,g,b,a;
+int val;
+int red=0;
+Ticker MC;
+int32_t act=101;
+std_msgs::Int32 pub_act;
+ros::NodeHandle nh;
+ros::Publisher pub_action("act_pose", &pub_act);
+void messageCallback(const std_msgs::Int32 &msg)
 {
-    led1=l1;
-    led2=l2;
-    led3=l3;
-    led4=l4;
+    act=msg.data;
 }
-
+void MCL()
+{
+    pub_action.publish(&pub_act);
+    nh.spinOnce();
+}
+//定点に到着した時にTz1:a,Tz2:b,Tz3:cの文字を足回りから送る。topicはshoot_action
+//装填用の信号はdとしておく。後で変更して、どうぞ。
+ros::Subscriber<std_msgs::Int32> sub("shoot_action",&messageCallback);
+ 
+//信号は一回しか受け取らないようにしておく。
 int main(int argc, char **argv)
 {
+    pub_act.data=0;
     nh.initNode();
     nh.subscribe(sub);
+    nh.advertise(pub_action);
+    MC.attach(&MCL,0.01);
+    pub_act.data=0;
+    char cmd[2];
+    char cell[1]= {0x03};
+    char data[8]= {0,0,0,0,0,0,0,0};
+    cmd[0] = 0x00;
+    cmd[1] = 0x89;
+    i2c.frequency(115200);
+    val = i2c.write(84, cmd, 2);
+    cmd[0] = 0x0;
+    cmd[1] = 0x09;
+    val = i2c.write(84, cmd, 2);
+    servo.period_ms(20);
+    //int jk=0;
     while(1) {
-        if((*(action.data)== 'k')&&(k_action==0)&&(o_action==0)) {
-            grab = 1;
-            o_action++;
-            led_on(0,1,0,0);
-            snap = 1;
-            wait(5);
-            grab = 0;
-            k_action++;
-            led_on(1,0,0,0);
+        //wait_ms(50);//最小値ではない
+        val = i2c.write(84, cell, 1);
+        val = i2c.read(84, data, 8);
+        red = data[0]<<8 | data[1];
+        //pc.printf("%d",red);
+        if((act==100)&&(ready_action==0)) {
+            snap=1;
+            grab=0;
+            if(red<RED_MIN){
+            grab=1;
+            s_grab=0;
+            ready_action=1;
+            count=0;
+            pub_act.data=act;
+            pub_action.publish(&pub_act);
+            }
+        }
+        if((act==2)&&(ready_action==0)) {
+            led2=1;
+            snap=1;
+            grab=0;
+            if(red<RED_MIN){
+            grab=1;
+            s_grab=0;
+            ready_action=1;
+            count=0;
+            pub_act.data=act;
+            pub_action.publish(&pub_act);
+            }
+        }
+        if((act==5)&&(ready_action==0)) {  
+            snap=1;
+            grab=0;
+            if(red<RED_MIN){
+            grab=1;
+            s_grab=0;
+            ready_action=1;
+            count=0;
+            pub_act.data=act;
+            pub_action.publish(&pub_act);
+            }
         }
-
-
-        else if((*(action.data)=='m')&&(m_action==0)) {
-            led_on(1,1,0,0);
-            snap = 0;
-            m_action++;
-            //pc.printf("a\n");
-            wait(0.195);             //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
-            grab = 1;
-            wait(3);
-            snap=1;
-
-        }
-        //一連の動作が終了したかを判断する。
-        if((k_action==1)&&(o_action==1)&&(m_action==1)) {
-            k_action=0;
-            o_action=0;
-            m_action=0;
-        }
-        nh.spinOnce();
+        //get ready
+        //shashutu
+            if((act== 1)&&(count==0)) { 
+                led1=1;
+                s_grab=1;
+                wait(3);
+                snap=0;
+                wait(0.17); //TZ1
+                grab=0;
+                ready_action=0;
+                count=1;
+                pub_act.data=act;
+                pub_action.publish(&pub_act);
+            }
+            if((act== 3)&&(count==0)) {
+                s_grab=1;
+                wait(3);
+                snap=0;
+                wait(0.17); //TZ1
+                grab=0;
+                ready_action=0;
+                count=1;
+                pub_act.data=act;
+                pub_action.publish(&pub_act);
+            }
+ 
+            if((act==4)&&(count==0)) {
+                led4=1;
+                s_grab=1;
+                wait(3);
+                snap=0;
+                wait(0.17); //TZ2
+                grab=0;
+                ready_action=0;
+                count=1;
+                pub_act.data=act;
+                pub_action.publish(&pub_act);
+            }
+            if((act==6)&&(count==0)) {
+                s_grab=1;
+                wait(3);
+                snap=0;
+                wait(0.17); //TZ2
+                grab=0;
+                ready_action=0;
+                count=1;
+                pub_act.data=act;
+                pub_action.publish(&pub_act);
+            }
+            if((act==7)&&(count==0)) {
+                s_grab=1;
+                wait(3);
+                snap=0;
+                wait(0.17); //TZ3
+                grab=0;
+                ready_action=0;
+                count=1;
+                pub_act.data=act;
+                pub_action.publish(&pub_act);
+            }
+            //一連の動作が終了したかを判断する。
+            if(count>0) {
+                //count=0;
+                have_cock=0;
+                have_action=0;
+                //have_cock=0;
+                //ready_action=0;
+            }
+            //if(ready_action>0){
+            //    count=0;
+            //    }
+        
+        //if(pub_act.data!=act)pub_act.data=0;
     }
-
+ 
 }
\ No newline at end of file