action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
main.cpp@1:d6bbefff36fa, 2021-07-05 (annotated)
- Committer:
- mondiaye
- Date:
- Mon Jul 05 16:58:26 2021 +0000
- Revision:
- 1:d6bbefff36fa
- Child:
- 2:9847e6e7ce2d
Starting point
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mondiaye | 1:d6bbefff36fa | 1 | /* |
mondiaye | 1:d6bbefff36fa | 2 | * rosserial Publisher Example |
mondiaye | 1:d6bbefff36fa | 3 | * Prints "hello world!" |
mondiaye | 1:d6bbefff36fa | 4 | */ |
mondiaye | 1:d6bbefff36fa | 5 | |
mondiaye | 1:d6bbefff36fa | 6 | #include"mbed.h" |
mondiaye | 1:d6bbefff36fa | 7 | #include <ros.h> |
mondiaye | 1:d6bbefff36fa | 8 | #include <std_msgs/String.h> |
mondiaye | 1:d6bbefff36fa | 9 | #include <Gobeur.h> |
mondiaye | 1:d6bbefff36fa | 10 | #include <gobeur_lib.h> |
mondiaye | 1:d6bbefff36fa | 11 | #include <Servo.h> |
mondiaye | 1:d6bbefff36fa | 12 | |
mondiaye | 1:d6bbefff36fa | 13 | Servo gobeur_left_aspire_servo(servo_aspire_1_pin); |
mondiaye | 1:d6bbefff36fa | 14 | Servo gobeur_right_aspire_servo(servo_aspire_2_pin); |
mondiaye | 1:d6bbefff36fa | 15 | Servo gobeur_left_block_servo(servo_block_1_pin); |
mondiaye | 1:d6bbefff36fa | 16 | Servo gobeur_right_block_servo(servo_block_2_pin); |
mondiaye | 1:d6bbefff36fa | 17 | |
mondiaye | 1:d6bbefff36fa | 18 | |
mondiaye | 1:d6bbefff36fa | 19 | void gobeur_initialize() |
mondiaye | 1:d6bbefff36fa | 20 | { |
mondiaye | 1:d6bbefff36fa | 21 | |
mondiaye | 1:d6bbefff36fa | 22 | } |
mondiaye | 1:d6bbefff36fa | 23 | |
mondiaye | 1:d6bbefff36fa | 24 | int aspirateur_on(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 25 | { |
mondiaye | 1:d6bbefff36fa | 26 | if(gobeur_id == 0) |
mondiaye | 1:d6bbefff36fa | 27 | gobeur_right_aspire_servo = 20; |
mondiaye | 1:d6bbefff36fa | 28 | if(gobeur_id == 1) |
mondiaye | 1:d6bbefff36fa | 29 | gobeur_left_aspire_servo = 20; |
mondiaye | 1:d6bbefff36fa | 30 | } |
mondiaye | 1:d6bbefff36fa | 31 | |
mondiaye | 1:d6bbefff36fa | 32 | int aspirateur_off(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 33 | { |
mondiaye | 1:d6bbefff36fa | 34 | if(gobeur_id == 0) |
mondiaye | 1:d6bbefff36fa | 35 | gobeur_right_aspire_servo = 0; |
mondiaye | 1:d6bbefff36fa | 36 | // Pour activer en rampe |
mondiaye | 1:d6bbefff36fa | 37 | /* for(int i=0; i<100; i++) { |
mondiaye | 1:d6bbefff36fa | 38 | myservo = i/100.0; |
mondiaye | 1:d6bbefff36fa | 39 | wait(0.01); |
mondiaye | 1:d6bbefff36fa | 40 | }*/ |
mondiaye | 1:d6bbefff36fa | 41 | if(gobeur_id == 1) |
mondiaye | 1:d6bbefff36fa | 42 | gobeur_left_aspire_servo = 0; |
mondiaye | 1:d6bbefff36fa | 43 | } |
mondiaye | 1:d6bbefff36fa | 44 | |
mondiaye | 1:d6bbefff36fa | 45 | int block_gobelet(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 46 | { |
mondiaye | 1:d6bbefff36fa | 47 | if(gobeur_id == 0) |
mondiaye | 1:d6bbefff36fa | 48 | gobeur_right_block_servo = 20; |
mondiaye | 1:d6bbefff36fa | 49 | if(gobeur_id == 1) |
mondiaye | 1:d6bbefff36fa | 50 | gobeur_left_block_servo = 20; |
mondiaye | 1:d6bbefff36fa | 51 | } |
mondiaye | 1:d6bbefff36fa | 52 | int unblock_gobelet(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 53 | { |
mondiaye | 1:d6bbefff36fa | 54 | if(gobeur_id == 0) |
mondiaye | 1:d6bbefff36fa | 55 | gobeur_right_block_servo = 0; |
mondiaye | 1:d6bbefff36fa | 56 | if(gobeur_id == 1) |
mondiaye | 1:d6bbefff36fa | 57 | gobeur_left_block_servo = 0; |
mondiaye | 1:d6bbefff36fa | 58 | } |
mondiaye | 1:d6bbefff36fa | 59 | |
mondiaye | 1:d6bbefff36fa | 60 | void gobeur_action(const stm32_gobeur::Gobeur& gobeur_msg){ |
mondiaye | 1:d6bbefff36fa | 61 | gobeur_right_status = gobeur_msg.gobeur_right_status ; |
mondiaye | 1:d6bbefff36fa | 62 | gobeur_left_status = gobeur_msg.gobeur_left_status ; |
mondiaye | 1:d6bbefff36fa | 63 | gobeur_right_aspire = gobeur_msg.gobeur_right_aspire ; |
mondiaye | 1:d6bbefff36fa | 64 | gobeur_left_aspire = gobeur_msg.gobeur_left_aspire ; |
mondiaye | 1:d6bbefff36fa | 65 | game_status = gobeur_msg.game_status ; |
mondiaye | 1:d6bbefff36fa | 66 | |
mondiaye | 1:d6bbefff36fa | 67 | if ( gobeur_msg.init_status == 1 && game_status == 1 ) { |
mondiaye | 1:d6bbefff36fa | 68 | gobeur_initialize(); |
mondiaye | 1:d6bbefff36fa | 69 | } |
mondiaye | 1:d6bbefff36fa | 70 | } |
mondiaye | 1:d6bbefff36fa | 71 | |
mondiaye | 1:d6bbefff36fa | 72 | |
mondiaye | 1:d6bbefff36fa | 73 | ros::NodeHandle nh; |
mondiaye | 1:d6bbefff36fa | 74 | |
mondiaye | 1:d6bbefff36fa | 75 | std_msgs::String str_msg; |
mondiaye | 1:d6bbefff36fa | 76 | ros::Publisher chatter("chatter", &str_msg); |
mondiaye | 1:d6bbefff36fa | 77 | |
mondiaye | 1:d6bbefff36fa | 78 | ros::Subscriber<stm32_gobeur::Gobeur> sub("gobeur", &gobeur_action); |
mondiaye | 1:d6bbefff36fa | 79 | |
mondiaye | 1:d6bbefff36fa | 80 | char hello[13] = "hello!"; |
mondiaye | 1:d6bbefff36fa | 81 | |
mondiaye | 1:d6bbefff36fa | 82 | DigitalOut led = LED1; |
mondiaye | 1:d6bbefff36fa | 83 | |
mondiaye | 1:d6bbefff36fa | 84 | int main() { |
mondiaye | 1:d6bbefff36fa | 85 | nh.initNode(); |
mondiaye | 1:d6bbefff36fa | 86 | nh.advertise(chatter); |
mondiaye | 1:d6bbefff36fa | 87 | |
mondiaye | 1:d6bbefff36fa | 88 | while (1) { |
mondiaye | 1:d6bbefff36fa | 89 | led = !led; |
mondiaye | 1:d6bbefff36fa | 90 | |
mondiaye | 1:d6bbefff36fa | 91 | switch(gobeur_right_aspire) { |
mondiaye | 1:d6bbefff36fa | 92 | case 2 : aspirateur_on(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 93 | unblock_gobelet(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 94 | wait_ms(2000); |
mondiaye | 1:d6bbefff36fa | 95 | block_gobelet(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 96 | aspirateur_off(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 97 | break; |
mondiaye | 1:d6bbefff36fa | 98 | case 1 : aspirateur_off(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 99 | unblock_gobelet(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 100 | wait_ms(2000); |
mondiaye | 1:d6bbefff36fa | 101 | break; |
mondiaye | 1:d6bbefff36fa | 102 | case 0 : break; |
mondiaye | 1:d6bbefff36fa | 103 | } |
mondiaye | 1:d6bbefff36fa | 104 | |
mondiaye | 1:d6bbefff36fa | 105 | str_msg.data = hello; |
mondiaye | 1:d6bbefff36fa | 106 | chatter.publish( &str_msg ); |
mondiaye | 1:d6bbefff36fa | 107 | nh.spinOnce(); |
mondiaye | 1:d6bbefff36fa | 108 | wait_ms(1000); |
mondiaye | 1:d6bbefff36fa | 109 | } |
mondiaye | 1:d6bbefff36fa | 110 | } |