ROBOSTEP4期
/
NHK_kaida_ros0329
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed ros_lib_kinetic
Fork of
NHK_kaida_ros0322_4
by 
ROBOSTEP4期
Diff: main.cpp
- Revision:
- 11:5a06fd933e55
- Parent:
- 10:a05e9a8980db
- Child:
- 12:6832bc92272f
--- a/main.cpp Mon Mar 05 01:37:19 2018 +0000
+++ b/main.cpp Thu Mar 15 01:48:50 2018 +0000
@@ -2,7 +2,7 @@
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
-#define R_C_MAX 50
+#define R_C_MAX 2
Serial pc(USBTX,USBRX);
I2C i2c(p9, p10); // sda, scl
@@ -11,6 +11,11 @@
DigitalOut grab(p11); //0:開く 1:閉じる
DigitalOut snap(p14); //1:装填 0:発射
DigitalOut s_grab(p13); //0:解放 1:把持
+DigitalOut fack(p12); //0:突出 2:引抜
+DigitalOut grab2(p18); //0:開く 1:閉じる
+DigitalOut snap2(p15); //1:装填 0:発射
+DigitalOut s_grab2(p16); //0:解放 1:把持
+DigitalOut fack2(p17); //0:突出 2:引抜
DigitalOut ledw(p20); //射出LED
DigitalOut led1(LED1);
@@ -36,58 +41,118 @@
int RED_MIN=0;
int RED_MIN2=0;
int32_t act=101;
-int ball_judgement=0;
+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)
{
act=msg.data;
}
+
+void messageCallback2(const std_msgs::Int32 &msg)
+{
+ 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;
+ s_grab=0;
+ wait(2);
+ ledw=1;
+ wait(1);
+ snap=0;
+ wait(waittime);
+ grab=0;
+ wait(1);
+ ledw=0;
+ fack=0;
}
else if (act==4) {
led3=1;
+ s_grab2=0;
+ wait(2);
+ ledw=1;
+ wait(1);
+ snap2=0;
+ wait(waittime);
+ grab2=0;
+ wait(1);
+ ledw=0;
+ fack2=0;
}
else if (act==6) {
led2=1;
led3=1;
+ s_grab2=0;
+ wait(2);
+ ledw=1;
+ wait(1);
+ snap2=0;
+ wait(waittime);
+ grab2=0;
+ wait(1);
+ ledw=0;
+ fack2=0;
}
else if (act==7) {
led1=1;
led2=1;
led3=1;
+ s_grab=0;
+ s_grab2=0;
+ wait(2);
+ ledw=1;
+ wait(1);
+ snap=0;
+ snap2=0;
+ wait(waittime);
+ grab=0;
+ grab2=0;
+ wait(1);
+ ledw=0;
+ fack=0;
+ fack2=0;
}
-
- 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;
@@ -113,13 +178,17 @@
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;
- int r_c=0;
- int r_c2=0;
int first_up=0;
char cmd[2];
char cell[1]= {0x03};
@@ -155,101 +224,114 @@
///装填///
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(act==1) {
+ led1=1;
}
- if(red2<RED_MIN2) {
- r_c2++;
+ else if(act==3) {
+ led1=1;
+ led2=1;
+ }
+ else if(act==5) {
+ led1=1;
+ led3=1;
}
- else{
- r_c=0;
- r_c2=0;
+ else if(act==8) {
+ led4=1;
+ }*/
+ snap=1;
+ snap2=1;
+ grab=1;
+ grab2=1;
+ if(finding==1){
+ led1=1;
+ if(ball_judgement==1){
+ led2=1;
+ led3=0;
+ led4=0;
+ wait(1.5);//桃井escape
+ grab=1;
+ //wait(1);
+ s_grab=1;
+ wait(1);
+ fack=1;
+
}
- 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;
+ else if(ball_judgement==2){
+ led2=0;
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;
+ led4=0;
+ wait(1.5);//桃井escape
+ grab2=1;
+ //wait(1);
+ s_grab2=1;
+ wait(1);
+ fack2=1;
+
+ }
+ else if(ball_judgement==3){
led2=0;
- }
- else if(act==5) {
- led1=0;
led3=0;
- }
- else if(act==8) {
- led4=0;
- }
+ led4=1;
+ wait(1.5);//桃井escape
+ grab=1;
+ grab2=1;
+ //wait(1);
+ s_grab=1;
+ s_grab2=1;
+ wait(1);
+ fack=1;
+ fack2=1;
+
+ }
-
-
+ 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;
- }
+ 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
+ throw_cock(0.218); //TZ1
ready_action=0;
count=1;
first_up=0;
pub_act.data=act;
pub_action.publish(&pub_act);
+ pub_fin.data=1;
+ pub_finc.publish(&pub_fin);
}
if(((act==4)||(act==6))&&(count==0)) {
- throw_cock(0.205); //TZ2
+ throw_cock(0.218); //TZ2
ready_action=0;
count=1;
first_up=0;
pub_act.data=act;
pub_action.publish(&pub_act);
+ pub_fin.data=1;
+ pub_finc.publish(&pub_fin);
}
if((act==7)&&(count==0)) {
@@ -261,6 +343,8 @@
first_up=0;
pub_act.data=act;
pub_action.publish(&pub_act);
+ pub_fin.data=1;
+ pub_finc.publish(&pub_fin);
}
//一連の動作が終了したかを判断する。