![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
main.cpp
- Committer:
- mondiaye
- Date:
- 2021-07-05
- Revision:
- 2:9847e6e7ce2d
- Parent:
- 1:d6bbefff36fa
- Child:
- 3:a284535865b7
File content as of revision 2:9847e6e7ce2d:
/* * 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); 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; } int aspirateur_on(uint8_t gobeur_id) { if(gobeur_id == 0) gobeur_right_aspire_servo = 0.2; if(gobeur_id == 1) gobeur_left_aspire_servo = 0.2; } 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); }*/ if(gobeur_id == 1) gobeur_left_aspire_servo = 0; } int block_gobelet(uint8_t gobeur_id) { if(gobeur_id == 0) gobeur_right_block_servo = 0.2; if(gobeur_id == 1) gobeur_left_block_servo = 0.2; } int unblock_gobelet(uint8_t gobeur_id) { if(gobeur_id == 0) gobeur_right_block_servo = 0; if(gobeur_id == 1) gobeur_left_block_servo = 0; } void gobeur_action(const stm32_gobeur::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(); } } 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); while (1) { led = !led; switch(gobeur_right_aspire) { case 2 : aspirateur_on(GOBEUR_RIGHT); unblock_gobelet(GOBEUR_RIGHT); wait_ms(2000); block_gobelet(GOBEUR_RIGHT); aspirateur_off(GOBEUR_RIGHT); break; case 1 : aspirateur_off(GOBEUR_RIGHT); unblock_gobelet(GOBEUR_RIGHT); wait_ms(2000); break; case 0 : break; } str_msg.data = hello; chatter.publish( &str_msg ); nh.spinOnce(); wait_ms(1000); } }