File content as of revision 22:37a07f844778:

#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);
 
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
DigitalIn enable(p21);

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; //装填待機
int32_t act=101;
int ball_judgement=1;
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_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)
{
    snap=1;
    snap2=1;
    //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;

    while(1) {
    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
                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);
            }
        
    ///射出///
        if((act==2)&&(count==0)) {
            int f_i=0;
            //throw_cock(0.247); //TZ1
            throw_cock(0.242); //TZ1 03_29 0.247-down
 
            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.257); //TZ2
            throw_cock(0.238); //TZ2 03_29 0.257-tyou down
            
            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.227); //TZ3
            throw_cock(0.221); //TZ3 0.227-down
 
            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;
        }
    }
}