UTCoupe / Mbed 2 deprecated stm32_actuator_gobeur

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?

UserRevisionLine numberNew 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 }