File content as of revision 7:2827264ddf53:

#include <mbed.h>///////////////////////レーザーとカラーセンサーなしのwait受け渡し用
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#define RED_MIN 4000
 
//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);
DigitalOut led5(p20);
DigitalOut led6(p21);
DigitalOut led7(p22);
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)
{
    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) {
        //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;
            led5=1;
            led6=0;
            led7=0;
            if(red<RED_MIN){
            led5=0;
            led6=1;
            led7=0;
            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);
            }
        }
        //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;
    }
 
}