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