File content as of revision 10:a05e9a8980db:

#include <mbed.h>
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#define R_C_MAX 50
 
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 ledw(p20);   //射出LED
 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

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; 
 
Ticker MC;
std_msgs::Int32 pub_act;
std_msgs::Int32 pub_b;
ros::NodeHandle nh;
ros::Publisher pub_action("act_pose", &pub_act);
ros::Publisher pub_ball("act_ball", &pub_b);

void messageCallback(const std_msgs::Int32 &msg)
{
    act=msg.data;
}
void MCL()
{
    pub_b.data=ball_judgement;
    pub_action.publish(&pub_act);
    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);
 
/////射出サイクル/////
void throw_cock(float waittime)
{   
    if (act==2) {
        led2=1;
        }
    else if (act==4) {
        led3=1;
        }
    else if (act==6) {
        led2=1;
        led3=1;
        }
    else if (act==7) {
        led1=1;
        led2=1;
        led3=1;
        }
        
    s_grab=0;
    wait(2);
    ledw=1;
    wait(1);
    snap=0;
    wait(waittime); 
    grab=0;
    wait(1.2);///kinect用のwait
    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.advertise(pub_action);
    nh.advertise(pub_ball);
    MC.attach(&MCL,0.01);
    pub_act.data=0;
    pub_b.data=0;
    int r_c=0;
    int r_c2=0;
    int first_up=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==8))&&(ready_action==0)) { //TZ3
            if(first_up==0&&red!=0) {
                RED_MIN=red-150;
                RED_MIN2=red2-150;
                first_up++;
            }
            if(first_up>0) {
                snap=1;
                grab=0;
                if(red<RED_MIN) {
                    r_c++;
                }
                if(red2<RED_MIN2) {
                    r_c2++;
                }
                else{
                    r_c=0;
                    r_c2=0;
                }
                if(r_c>R_C_MAX||r_c2>R_C_MAX) {
                    if(ball_judgement==0){
                        if(r_c2>R_C_MAX)ball_judgement=1;
                    }
                    else if(ball_judgement==1){
                        if(r_c-r_c2>-R_C_MAX/2&&r_c-r_c2<R_C_MAX/2)ball_judgement=2;
                    }
                    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;
                        }
                            
                        
                    wait(1.5);
                    grab=1;
                    wait(1);
                    s_grab=1;
                    wait(2);
                    
                    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;
                        }
                    
                    
                    
                    
                    ready_action=1;
                    count=0;
                    pub_act.data=act;
                    pub_action.publish(&pub_act);
                    r_c=0;
                }
            }
        }
        
    ///射出///
        if((act==2)&&(count==0)) {
            
            throw_cock(0.21); //TZ1
 
            ready_action=0;
            count=1;
            first_up=0;
            pub_act.data=act;
            pub_action.publish(&pub_act);
        }
 
         if(((act==4)||(act==6))&&(count==0)) {
            
            throw_cock(0.205); //TZ2
 
            ready_action=0;
            count=1;
            first_up=0;
            pub_act.data=act;
            pub_action.publish(&pub_act);
        }
        
         if((act==7)&&(count==0)) {
 
            throw_cock(0.172); //TZ3
 
            ready_action=0;
            count=1;
            first_up=0;
            pub_act.data=act;
            pub_action.publish(&pub_act);
        }
        
        //一連の動作が終了したかを判断する。
        if(count>0) {
            have_cock=0;
            have_action=0;
        }
    }
}