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期
Revision 10:a05e9a8980db, committed 2018-03-05
- Comitter:
- Arare
- Date:
- Mon Mar 05 01:37:19 2018 +0000
- Parent:
- 9:1dcd40da31ec
- Child:
- 11:5a06fd933e55
- Commit message:
- ??
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Dec 02 04:14:02 2017 +0000
+++ b/main.cpp Mon Mar 05 01:37:19 2018 +0000
@@ -1,256 +1,272 @@
-#include <mbed.h>///////////////////////レーザーとカラーセンサーなしのwait受け渡し用
+#include <mbed.h>
#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
-int RED_MIN=0;
#define R_C_MAX 50
-//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
+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:otiru 1:ageru
+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 led5(p20);
-DigitalOut led6(p21);
-DigitalOut led7(p22);
+
+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 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;
+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)
{
- pub_act.data=0;
+ 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);
- //servo.period_ms(20);
- //int jk=0;
+ val2 = i2c_2.write(84, cmd, 2);
+
while(1) {
- //wait_ms(50);//最小値ではない
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];
- if(first_up==0&&red!=0){
- RED_MIN=red-500;
- first_up++;}
- if(red<RED_MIN){
+ red2 = data2[0]<<8 | data2[1];
+ /* if(red<RED_MIN) {
led1=1;
led2=0;
- //led5=1;
- //led6=1;
- //led7=1;
- }
- else{
+ } else {
led1=0;
led2=1;
- //led5=0;
- //led6=0;
- //led7=0;
- }
- //pc.printf("%d",red);
- if(first_up>0){
- if((act==100)&&(ready_action==0)) {
- snap=1;
- grab=0;
- if(red<RED_MIN){
- r_c++;
- }
- else r_c=0;
- if(r_c>R_C_MAX){
- grab=1;
- s_grab=0;
- ready_action=1;
- count=0;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- r_c=0;
+ }*/
+
+ //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((act==2)&&(ready_action==0)) {
- //led2=1;
- snap=1;
- grab=0;
- if(red<RED_MIN){
- r_c++;
- }
- else r_c=0;
- if(r_c>R_C_MAX){
- grab=1;
- s_grab=0;
- ready_action=1;
- count=0;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- r_c=0;
- }
- }
- if((act==5)&&(ready_action==0)) {
- snap=1;
- grab=0;
- if(red<RED_MIN){
- r_c++;
- }
- else r_c=0;
- if(r_c>R_C_MAX){
- grab=1;
- s_grab=0;
- ready_action=1;
- count=0;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- r_c=0;
+ 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;
+ }
}
}
- //get ready
- //shashutu
- if((act== 1)&&(count==0)) {
- //led1=1;
- //led6=1;
- //led7=1;
- s_grab=1;
- wait(1);
- led5=1;
- wait(9);
- snap=0;
- wait(0.183); //TZ1
- grab=0;
- ready_action=0;
- count=1;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- led5=0;
- //led6=0;
- //led7=0;
- }
- if((act== 3)&&(count==0)) {
- //led6=1;
- //led7=1;
- s_grab=1;
- wait(1);
- led5=1;
- wait(9);
- snap=0;
- wait(0.183); //TZ1
- grab=0;
- ready_action=0;
- count=1;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- led5=0;
- //led6=0;
- //led7=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
- if((act==4)&&(count==0)) {
- //led4=1;
- //led6=1;
- //led7=1;
- s_grab=1;
- wait(1);
- led5=1;
- wait(9);
- snap=0;
- wait(0.182); //TZ2
- grab=0;
- ready_action=0;
- count=1;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- led5=0;
- //led6=0;
- //led7=0;
- }
- if((act==6)&&(count==0)) {
- s_grab=1;
- wait(1);
- led5=1;
- wait(9);
- snap=0;
- wait(0.182); //TZ2
- grab=0;
- ready_action=0;
- count=1;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- led5=0;
- }
- if((act==7)&&(count==0)) {
- s_grab=1;
- wait(1);
- led5=1;
- wait(9);
- snap=0;
- wait(0.152); //TZ3
- grab=0;
- ready_action=0;
- count=1;
- pub_act.data=act;
- pub_action.publish(&pub_act);
- led5=0;
- }
- //一連の動作が終了したかを判断する。
- if(count>0) {
- //count=0;
- have_cock=0;
- have_action=0;
- //have_cock=0;
- //ready_action=0;
- }
- //if(ready_action>0){
- // count=0;
- // }
+ ready_action=0;
+ count=1;
+ first_up=0;
+ pub_act.data=act;
+ pub_action.publish(&pub_act);
+ }
- //if(pub_act.data!=act)pub_act.data=0;
+ 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;
+ }
}
- }
-}
\ No newline at end of file
+}