File content as of revision 20:81a956eaf4ae:

#include <mbed.h>
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#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:発射
DigitalOut s_grab(p13); //0:解放 1:把持
DigitalOut fuck(p17);   //0:引抜 1:突出
DigitalOut grab2(p18);   //0:開く 1:閉じる
DigitalOut snap2(p15);   //1:装填 0:発射
DigitalOut s_grab2(p16); //0:解放 1:把持
DigitalOut ledw(p20);   //射出LED
 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut LEDN(p10);
DigitalOut out(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;
int val2;
int red2=0;
int RED_MIN=0;
int RED_MIN2=0;
int32_t act=101;
int ball_judgement=0;
int r_c=0;
int r_c2=0;
int finding=0;
 
Ticker MC;
std_msgs::Int32 pub_act;
std_msgs::Int32 pub_b;
std_msgs::Int32 pub_c1;
std_msgs::Int32 pub_c2;
std_msgs::Int32 pub_fin;
ros::NodeHandle nh;
ros::Publisher pub_action("act_pose", &pub_act);
ros::Publisher pub_ball("act_ball", &pub_b);
ros::Publisher pub_color1("act_color1", &pub_c1);
ros::Publisher pub_color2("act_color2", &pub_c2);
ros::Publisher pub_finc("shoot_fin", &pub_fin);

void messageCallback(const std_msgs::Int32 &msg)
{
    if(act!=9&&msg.data==9){
        ready_action=1;
        count=0;
    }
    act=msg.data;
}

void messageCallback2(const std_msgs::Int32 &msg)
{
    if((act==1)||(act==3)||(act==5)||(act==10)||(act==11))ball_judgement=msg.data;
}

void messageCallback3(const std_msgs::Int32 &msg)
{
    finding=msg.data;
}

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);
    pub_ball.publish(&pub_b);
    nh.spinOnce();
}
//定点に到着した時にTz1:a,Tz2:b,Tz3:cの文字を足回りから送る。topicはshoot_action
//装填用の信号はdとしておく。後で変更して、どうぞ。
ros::Subscriber<std_msgs::Int32> sub("shoot_action",&messageCallback);
ros::Subscriber<std_msgs::Int32> sub_TZ("tz",&messageCallback2);
ros::Subscriber<std_msgs::Int32> sub_find("find",&messageCallback3);
 
/////射出サイクル/////
void throw_cock(float waittime)
{   
    if (act==2) {
        led2=1;
        fuck=1;
        s_grab=0;
        wait(2);
        ledw=1;
        wait(1);
        snap=0;
        wait(waittime); 
        grab=0;
        wait(1);
        ledw=0;
        }
    else if (act==4) {
        led3=1;
        fuck=1;
        s_grab2=0;
        wait(2);
        ledw=1;
        wait(1);
        snap2=0;
        wait(waittime); 
        grab2=0;
        wait(1);
        ledw=0;
        }
    else if (act==6) {
        led2=1;
        led3=1;
        fuck=1;
        s_grab2=0;
        wait(2);
        ledw=1;
        wait(1);
        snap2=0;
        wait(waittime); 
        grab2=0;
        wait(1);
        ledw=0;
        }
    else if (act==7) {
        fuck=1;
        s_grab=0;
        wait(2);
        ledw=1;
        wait(1);
        snap=0;
        wait(waittime); 
        grab=0;
        wait(1);
        ledw=0;
        }
    else if (act==8) {
        fuck=1;
        s_grab2=0;
        wait(2);
        ledw=1;
        wait(1);
        snap2=0;
        wait(waittime); 
        grab2=0;
        wait(1);
        ledw=0;
        }
    else if (act==9) {
        fuck=1;
        s_grab2=0;
        wait(2);
        ledw=1;
        wait(1);
        snap2=0;
        wait(waittime); 
        grab2=0;
        wait(1);
        ledw=0;
        }
    
    if (act==2) {
        led2=0;
        }
    else if (act==4) {
        led3=0;
        }
    else if (act==6) {
        led2=0;
        led3=0;
        }
    else if (act==7) {
        led1=0;
        led2=0;
        led3=0;
        }
}
 
 
int main(int argc, char **argv)
{
    out=1;
    
    nh.initNode();
    nh.subscribe(sub);
    nh.subscribe(sub_TZ);
    nh.subscribe(sub_find);
    nh.advertise(pub_action);
    nh.advertise(pub_ball);
    nh.advertise(pub_color1);
    nh.advertise(pub_color2);
    nh.advertise(pub_finc);
    MC.attach(&MCL,0.01);
    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(((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;
                grab2=0;
                fuck=0;
                s_grab=0;
                s_grab2=0;
                while(finding!=1){
                    led1=0;
                    led2=0;
                    led3=0;
                    led4=0;
                    LEDN=1;
                }
                if(ball_judgement==1){ 
                    led1=0;
                    led2=1;
                    led3=0;
                    led4=0;
                    wait(2);//桃井escape    
                    grab=1;
                    wait(0.5);
                    s_grab=1;
                    wait(0.5);
                    fuck=0;
                } 
                else if(ball_judgement==2){ 
                    led1=0;
                    led2=0;
                    led3=1;
                    led4=0;
                    
                    wait(2);//桃井escape
                    grab2=1;
                    wait(0.5);
                    s_grab2=1;
                    wait(0.5);
                    fuck=0;
                }
                else if(ball_judgement>=3){ 
                    led1=0;
                    led2=0;
                    led3=0;
                    led4=1;
                    wait(2);//桃井escape
                    grab=1;
                    grab2=1;
                    wait(0.5);
                    s_grab=1;
                    s_grab2=1;
                    wait(0.5);
                    fuck=0;
                }
                else{
                    led1=1;
                    led2=0;
                    led3=0;
                    led4=0;
                    }
                if(act==1) {
                    led1=0;
                }
                else if(act==3) {
                    led1=0;
                    led2=0;
                }
                else if(act==5) {
                    led1=0;
                    led3=0;
                }
                else if(act==8) {
                    led4=0;
                }    
                LEDN=0;    
                ready_action=1;
                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.257); //TZ1
 
            ready_action=0;
            count=1;
            pub_act.data=act;
            pub_action.publish(&pub_act);
            for(f_i=0;f_i<20;f_i++){
                pub_fin.data=1;
                pub_finc.publish(&pub_fin);
                wait(0.01);
            }
        }
 
         if(((act==4)||(act==6))&&(count==0)) {
             int f_i=0;
            throw_cock(0.258); //TZ2
 
            ready_action=0;
            count=1;
            pub_act.data=act;
            pub_action.publish(&pub_act);
            for(f_i=0;f_i<20;f_i++){
                pub_fin.data=1;
                pub_finc.publish(&pub_fin);
                wait(0.01);
            }
        }
        
         if(((act==7)||(act==8)||(act==9))&&(count==0))  {
            int f_i=0;
            throw_cock(0.228); //TZ3
 
            ready_action=0;
            count=1;
            pub_act.data=act;
            pub_action.publish(&pub_act);
            for(f_i=0;f_i<20;f_i++){
                pub_fin.data=1;
                pub_finc.publish(&pub_fin);
                wait(0.01);
            }
        }
        
        //一連の動作が終了したかを判断する。
        if(count>0) {
            have_cock=0;
            have_action=0;
        }
    }
}