action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
main.cpp
- Committer:
- mondiaye
- Date:
- 2021-07-05
- Revision:
- 3:a284535865b7
- Parent:
- 2:9847e6e7ce2d
- Child:
- 4:851261a9273b
File content as of revision 3:a284535865b7:
/* * rosserial Publisher Example * Prints "hello world!" */ #include"mbed.h" #include <ros.h> #include <std_msgs/String.h> #include <Gobeur.h> #include <gobeur_lib.h> #include <Servo.h> Servo gobeur_left_aspire_servo(servo_aspire_1_pin); Servo gobeur_right_aspire_servo(servo_aspire_2_pin); Servo gobeur_left_block_servo(servo_block_1_pin); Servo gobeur_right_block_servo(servo_block_2_pin); ros::NodeHandle nh; std_msgs::String str_msg; ros::Publisher chatter("chatter", &str_msg); ros::Subscriber<gobeur_node::Gobeur> sub("gobeur", &gobeur_action); char hello[13] = "hello!"; void gobeur_initialize() { gobeur_right_aspire_servo = 0; gobeur_left_aspire_servo = 0; wait(2); gobeur_right_aspire_servo = 1.0; gobeur_left_aspire_servo = 1.0; wait(2); gobeur_right_aspire_servo = 0; gobeur_left_aspire_servo = 0; wait(2); nh.loginfo("Actuators initialised"); } int aspirateur_on(uint8_t gobeur_id) { if(gobeur_id == 0) { gobeur_right_aspire_servo = 0.15; nh.loginfo("Aspirateur right on"); } if(gobeur_id == 1) { gobeur_left_aspire_servo = 0.15; nh.loginfo("Aspirateur left on"); } } int aspirateur_off(uint8_t gobeur_id) { if(gobeur_id == 0) { gobeur_right_aspire_servo = 0.0; nh.loginfo("Aspirateur right off"); } if(gobeur_id == 1) { gobeur_left_aspire_servo = 0.0; nh.loginfo("Aspirateur left off"); } } int block_gobelet(uint8_t gobeur_id) { if(gobeur_id == 0) { gobeur_right_block_servo = 0.15; nh.loginfo("Gobelet right blocked"); } if(gobeur_id == 1) { gobeur_left_block_servo = 0.15; nh.loginfo("Gobelet left blocked"); } } int unblock_gobelet(uint8_t gobeur_id) { { gobeur_right_block_servo = 0.0; nh.loginfo("Gobelet right unblocked"); } if(gobeur_id == 1) { gobeur_left_block_servo = 0.0; nh.loginfo("Gobelet left unblocked"); } } void gobeur_action(const gobeur_node::Gobeur& gobeur_msg){ gobeur_right_status = gobeur_msg.gobeur_right_status ; gobeur_left_status = gobeur_msg.gobeur_left_status ; gobeur_right_aspire = gobeur_msg.gobeur_right_aspire ; gobeur_left_aspire = gobeur_msg.gobeur_left_aspire ; game_status = gobeur_msg.game_status ; if ( gobeur_msg.init_status == 1 && game_status == 1 ) { gobeur_initialize(); } } int main() { nh.initNode(); nh.advertise(chatter); nh.subscribe(sub); str_msg.data = hello; chatter.publish( &str_msg ); while (1) { switch(gobeur_right_aspire) { case 2 : unblock_gobelet(GOBEUR_RIGHT); aspirateur_on(GOBEUR_RIGHT); wait(2); block_gobelet(GOBEUR_RIGHT); aspirateur_off(GOBEUR_RIGHT); gobeur_right_aspire = 0; break; case 1 : aspirateur_off(GOBEUR_RIGHT); unblock_gobelet(GOBEUR_RIGHT); gobeur_right_aspire = 0; break; case 0 : break; } switch(gobeur_right_status) { case 2: aspirateur_on(GOBEUR_RIGHT); break; case 1 : aspirateur_off(GOBEUR_RIGHT); break; } gobeur_right_status = -1; nh.spinOnce(); } }