File content as of revision 2:9847e6e7ce2d:

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

#include"mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <Gobeur.h>
#include <gobeur_lib.h>
#include <Servo.h>

Servo gobeur_left_aspire_servo(servo_aspire_1_pin);
Servo gobeur_right_aspire_servo(servo_aspire_2_pin);
Servo gobeur_left_block_servo(servo_block_1_pin);
Servo gobeur_right_block_servo(servo_block_2_pin);
 

void gobeur_initialize()
{
    gobeur_right_aspire_servo = 0;
    gobeur_left_aspire_servo = 0;
    wait(2);
    gobeur_right_aspire_servo = 1.0;
    gobeur_left_aspire_servo = 1.0;
    wait(2);
    gobeur_right_aspire_servo = 0;
    gobeur_left_aspire_servo = 0;
    }
    
int aspirateur_on(uint8_t gobeur_id) 
{
    if(gobeur_id == 0)
        gobeur_right_aspire_servo = 0.2;
    if(gobeur_id == 1)
        gobeur_left_aspire_servo = 0.2;
}

int aspirateur_off(uint8_t gobeur_id)
{
    if(gobeur_id == 0)
        gobeur_right_aspire_servo = 0;
        // Pour activer en rampe
         /*       for(int i=0; i<100; i++) { 
            myservo = i/100.0;
            wait(0.01);
        }*/
    if(gobeur_id == 1)
        gobeur_left_aspire_servo  = 0;
}

int block_gobelet(uint8_t gobeur_id) 
{
    if(gobeur_id == 0)
        gobeur_right_block_servo = 0.2;
    if(gobeur_id == 1)
        gobeur_left_block_servo = 0.2;
}
int unblock_gobelet(uint8_t gobeur_id)
{
    if(gobeur_id == 0)
        gobeur_right_block_servo = 0;
    if(gobeur_id == 1)
        gobeur_left_block_servo = 0;
}

void gobeur_action(const stm32_gobeur::Gobeur& gobeur_msg){
    gobeur_right_status   = gobeur_msg.gobeur_right_status ; 
    gobeur_left_status  = gobeur_msg.gobeur_left_status ; 
    gobeur_right_aspire = gobeur_msg.gobeur_right_aspire ; 
    gobeur_left_aspire = gobeur_msg.gobeur_left_aspire ; 
    game_status = gobeur_msg.game_status ; 
    
    if ( gobeur_msg.init_status == 1 && game_status == 1 ) {
        gobeur_initialize(); 
    }
}

    
ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

ros::Subscriber<stm32_gobeur::Gobeur> sub("gobeur", &gobeur_action);

char hello[13] = "hello!";

DigitalOut led = LED1;

int main() {
    nh.initNode();
    nh.advertise(chatter);

    while (1) {
        led = !led;
        
        switch(gobeur_right_aspire) {
            case 2 : aspirateur_on(GOBEUR_RIGHT);
                     unblock_gobelet(GOBEUR_RIGHT);
                     wait_ms(2000);
                     block_gobelet(GOBEUR_RIGHT);
                     aspirateur_off(GOBEUR_RIGHT);
                     break;      
            case 1 : aspirateur_off(GOBEUR_RIGHT);
                     unblock_gobelet(GOBEUR_RIGHT); 
                     wait_ms(2000);
                     break;      
            case 0 : break;
            }
        
        str_msg.data = hello;
        chatter.publish( &str_msg );
        nh.spinOnce();
        wait_ms(1000);
    }
}