action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
Diff: main.cpp
- Revision:
- 3:a284535865b7
- Parent:
- 2:9847e6e7ce2d
- Child:
- 4:851261a9273b
--- a/main.cpp Mon Jul 05 17:30:13 2021 +0000 +++ b/main.cpp Mon Jul 05 19:50:56 2021 +0000 @@ -15,6 +15,14 @@ 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() { @@ -26,45 +34,67 @@ 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.2; + { + gobeur_right_aspire_servo = 0.15; + nh.loginfo("Aspirateur right on"); + } + if(gobeur_id == 1) - gobeur_left_aspire_servo = 0.2; + { + 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; - // Pour activer en rampe - /* for(int i=0; i<100; i++) { - myservo = i/100.0; - wait(0.01); - }*/ + { + gobeur_right_aspire_servo = 0.0; + nh.loginfo("Aspirateur right off"); + } + if(gobeur_id == 1) - gobeur_left_aspire_servo = 0; + { + 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.2; + { + gobeur_right_block_servo = 0.15; + nh.loginfo("Gobelet right blocked"); + } if(gobeur_id == 1) - gobeur_left_block_servo = 0.2; + { + gobeur_left_block_servo = 0.15; + nh.loginfo("Gobelet left blocked"); + } } int unblock_gobelet(uint8_t gobeur_id) { - if(gobeur_id == 0) - gobeur_right_block_servo = 0; + { + gobeur_right_block_servo = 0.0; + nh.loginfo("Gobelet right unblocked"); + } if(gobeur_id == 1) - gobeur_left_block_servo = 0; + { + gobeur_left_block_servo = 0.0; + nh.loginfo("Gobelet left unblocked"); + } } -void gobeur_action(const stm32_gobeur::Gobeur& gobeur_msg){ +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 ; @@ -76,42 +106,38 @@ } } - -ros::NodeHandle nh; - -std_msgs::String str_msg; -ros::Publisher chatter("chatter", &str_msg); - -ros::Subscriber<stm32_gobeur::Gobeur> sub("gobeur", &gobeur_action); - -char hello[13] = "hello!"; - -DigitalOut led = LED1; - int main() { nh.initNode(); nh.advertise(chatter); - + nh.subscribe(sub); + + str_msg.data = hello; + chatter.publish( &str_msg ); + while (1) { - led = !led; switch(gobeur_right_aspire) { - case 2 : aspirateur_on(GOBEUR_RIGHT); - unblock_gobelet(GOBEUR_RIGHT); - wait_ms(2000); + 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); - wait_ms(2000); + gobeur_right_aspire = 0; break; case 0 : break; } - - str_msg.data = hello; - chatter.publish( &str_msg ); + + switch(gobeur_right_status) { + case 2: aspirateur_on(GOBEUR_RIGHT); + break; + case 1 : aspirateur_off(GOBEUR_RIGHT); + break; + } + gobeur_right_status = -1; nh.spinOnce(); - wait_ms(1000); } } \ No newline at end of file