UTCoupe / Mbed 2 deprecated stm32_actuator_gobeur

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?

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