action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
main.cpp@3:a284535865b7, 2021-07-05 (annotated)
- Committer:
- mondiaye
- Date:
- Mon Jul 05 19:50:56 2021 +0000
- Revision:
- 3:a284535865b7
- Parent:
- 2:9847e6e7ce2d
- Child:
- 4:851261a9273b
logs added and loops fixed
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 | 3:a284535865b7 | 18 | ros::NodeHandle nh; |
mondiaye | 3:a284535865b7 | 19 | |
mondiaye | 3:a284535865b7 | 20 | std_msgs::String str_msg; |
mondiaye | 3:a284535865b7 | 21 | ros::Publisher chatter("chatter", &str_msg); |
mondiaye | 3:a284535865b7 | 22 | |
mondiaye | 3:a284535865b7 | 23 | ros::Subscriber<gobeur_node::Gobeur> sub("gobeur", &gobeur_action); |
mondiaye | 3:a284535865b7 | 24 | |
mondiaye | 3:a284535865b7 | 25 | char hello[13] = "hello!"; |
mondiaye | 1:d6bbefff36fa | 26 | |
mondiaye | 1:d6bbefff36fa | 27 | void gobeur_initialize() |
mondiaye | 1:d6bbefff36fa | 28 | { |
mondiaye | 2:9847e6e7ce2d | 29 | gobeur_right_aspire_servo = 0; |
mondiaye | 2:9847e6e7ce2d | 30 | gobeur_left_aspire_servo = 0; |
mondiaye | 2:9847e6e7ce2d | 31 | wait(2); |
mondiaye | 2:9847e6e7ce2d | 32 | gobeur_right_aspire_servo = 1.0; |
mondiaye | 2:9847e6e7ce2d | 33 | gobeur_left_aspire_servo = 1.0; |
mondiaye | 2:9847e6e7ce2d | 34 | wait(2); |
mondiaye | 2:9847e6e7ce2d | 35 | gobeur_right_aspire_servo = 0; |
mondiaye | 2:9847e6e7ce2d | 36 | gobeur_left_aspire_servo = 0; |
mondiaye | 3:a284535865b7 | 37 | wait(2); |
mondiaye | 3:a284535865b7 | 38 | nh.loginfo("Actuators initialised"); |
mondiaye | 1:d6bbefff36fa | 39 | } |
mondiaye | 1:d6bbefff36fa | 40 | |
mondiaye | 1:d6bbefff36fa | 41 | int aspirateur_on(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 42 | { |
mondiaye | 1:d6bbefff36fa | 43 | if(gobeur_id == 0) |
mondiaye | 3:a284535865b7 | 44 | { |
mondiaye | 3:a284535865b7 | 45 | gobeur_right_aspire_servo = 0.15; |
mondiaye | 3:a284535865b7 | 46 | nh.loginfo("Aspirateur right on"); |
mondiaye | 3:a284535865b7 | 47 | } |
mondiaye | 3:a284535865b7 | 48 | |
mondiaye | 1:d6bbefff36fa | 49 | if(gobeur_id == 1) |
mondiaye | 3:a284535865b7 | 50 | { |
mondiaye | 3:a284535865b7 | 51 | gobeur_left_aspire_servo = 0.15; |
mondiaye | 3:a284535865b7 | 52 | nh.loginfo("Aspirateur left on"); |
mondiaye | 3:a284535865b7 | 53 | } |
mondiaye | 1:d6bbefff36fa | 54 | } |
mondiaye | 1:d6bbefff36fa | 55 | |
mondiaye | 1:d6bbefff36fa | 56 | int aspirateur_off(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 57 | { |
mondiaye | 1:d6bbefff36fa | 58 | if(gobeur_id == 0) |
mondiaye | 3:a284535865b7 | 59 | { |
mondiaye | 3:a284535865b7 | 60 | gobeur_right_aspire_servo = 0.0; |
mondiaye | 3:a284535865b7 | 61 | nh.loginfo("Aspirateur right off"); |
mondiaye | 3:a284535865b7 | 62 | } |
mondiaye | 3:a284535865b7 | 63 | |
mondiaye | 1:d6bbefff36fa | 64 | if(gobeur_id == 1) |
mondiaye | 3:a284535865b7 | 65 | { |
mondiaye | 3:a284535865b7 | 66 | gobeur_left_aspire_servo = 0.0; |
mondiaye | 3:a284535865b7 | 67 | nh.loginfo("Aspirateur left off"); |
mondiaye | 3:a284535865b7 | 68 | } |
mondiaye | 1:d6bbefff36fa | 69 | } |
mondiaye | 1:d6bbefff36fa | 70 | |
mondiaye | 1:d6bbefff36fa | 71 | int block_gobelet(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 72 | { |
mondiaye | 1:d6bbefff36fa | 73 | if(gobeur_id == 0) |
mondiaye | 3:a284535865b7 | 74 | { |
mondiaye | 3:a284535865b7 | 75 | gobeur_right_block_servo = 0.15; |
mondiaye | 3:a284535865b7 | 76 | nh.loginfo("Gobelet right blocked"); |
mondiaye | 3:a284535865b7 | 77 | } |
mondiaye | 1:d6bbefff36fa | 78 | if(gobeur_id == 1) |
mondiaye | 3:a284535865b7 | 79 | { |
mondiaye | 3:a284535865b7 | 80 | gobeur_left_block_servo = 0.15; |
mondiaye | 3:a284535865b7 | 81 | nh.loginfo("Gobelet left blocked"); |
mondiaye | 3:a284535865b7 | 82 | } |
mondiaye | 1:d6bbefff36fa | 83 | } |
mondiaye | 1:d6bbefff36fa | 84 | int unblock_gobelet(uint8_t gobeur_id) |
mondiaye | 1:d6bbefff36fa | 85 | { |
mondiaye | 3:a284535865b7 | 86 | { |
mondiaye | 3:a284535865b7 | 87 | gobeur_right_block_servo = 0.0; |
mondiaye | 3:a284535865b7 | 88 | nh.loginfo("Gobelet right unblocked"); |
mondiaye | 3:a284535865b7 | 89 | } |
mondiaye | 1:d6bbefff36fa | 90 | if(gobeur_id == 1) |
mondiaye | 3:a284535865b7 | 91 | { |
mondiaye | 3:a284535865b7 | 92 | gobeur_left_block_servo = 0.0; |
mondiaye | 3:a284535865b7 | 93 | nh.loginfo("Gobelet left unblocked"); |
mondiaye | 3:a284535865b7 | 94 | } |
mondiaye | 1:d6bbefff36fa | 95 | } |
mondiaye | 1:d6bbefff36fa | 96 | |
mondiaye | 3:a284535865b7 | 97 | void gobeur_action(const gobeur_node::Gobeur& gobeur_msg){ |
mondiaye | 1:d6bbefff36fa | 98 | gobeur_right_status = gobeur_msg.gobeur_right_status ; |
mondiaye | 1:d6bbefff36fa | 99 | gobeur_left_status = gobeur_msg.gobeur_left_status ; |
mondiaye | 1:d6bbefff36fa | 100 | gobeur_right_aspire = gobeur_msg.gobeur_right_aspire ; |
mondiaye | 1:d6bbefff36fa | 101 | gobeur_left_aspire = gobeur_msg.gobeur_left_aspire ; |
mondiaye | 1:d6bbefff36fa | 102 | game_status = gobeur_msg.game_status ; |
mondiaye | 1:d6bbefff36fa | 103 | |
mondiaye | 1:d6bbefff36fa | 104 | if ( gobeur_msg.init_status == 1 && game_status == 1 ) { |
mondiaye | 1:d6bbefff36fa | 105 | gobeur_initialize(); |
mondiaye | 1:d6bbefff36fa | 106 | } |
mondiaye | 1:d6bbefff36fa | 107 | } |
mondiaye | 1:d6bbefff36fa | 108 | |
mondiaye | 1:d6bbefff36fa | 109 | int main() { |
mondiaye | 1:d6bbefff36fa | 110 | nh.initNode(); |
mondiaye | 1:d6bbefff36fa | 111 | nh.advertise(chatter); |
mondiaye | 3:a284535865b7 | 112 | nh.subscribe(sub); |
mondiaye | 3:a284535865b7 | 113 | |
mondiaye | 3:a284535865b7 | 114 | str_msg.data = hello; |
mondiaye | 3:a284535865b7 | 115 | chatter.publish( &str_msg ); |
mondiaye | 3:a284535865b7 | 116 | |
mondiaye | 1:d6bbefff36fa | 117 | while (1) { |
mondiaye | 1:d6bbefff36fa | 118 | |
mondiaye | 1:d6bbefff36fa | 119 | switch(gobeur_right_aspire) { |
mondiaye | 3:a284535865b7 | 120 | case 2 : unblock_gobelet(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 121 | aspirateur_on(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 122 | wait(2); |
mondiaye | 1:d6bbefff36fa | 123 | block_gobelet(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 124 | aspirateur_off(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 125 | gobeur_right_aspire = 0; |
mondiaye | 1:d6bbefff36fa | 126 | break; |
mondiaye | 1:d6bbefff36fa | 127 | case 1 : aspirateur_off(GOBEUR_RIGHT); |
mondiaye | 1:d6bbefff36fa | 128 | unblock_gobelet(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 129 | gobeur_right_aspire = 0; |
mondiaye | 1:d6bbefff36fa | 130 | break; |
mondiaye | 1:d6bbefff36fa | 131 | case 0 : break; |
mondiaye | 1:d6bbefff36fa | 132 | } |
mondiaye | 3:a284535865b7 | 133 | |
mondiaye | 3:a284535865b7 | 134 | switch(gobeur_right_status) { |
mondiaye | 3:a284535865b7 | 135 | case 2: aspirateur_on(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 136 | break; |
mondiaye | 3:a284535865b7 | 137 | case 1 : aspirateur_off(GOBEUR_RIGHT); |
mondiaye | 3:a284535865b7 | 138 | break; |
mondiaye | 3:a284535865b7 | 139 | } |
mondiaye | 3:a284535865b7 | 140 | gobeur_right_status = -1; |
mondiaye | 1:d6bbefff36fa | 141 | nh.spinOnce(); |
mondiaye | 1:d6bbefff36fa | 142 | } |
mondiaye | 1:d6bbefff36fa | 143 | } |