UTCoupe / Mbed 2 deprecated stm32_actuator_gobeur

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?

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