Diff: main.cpp

Revision:

3:a284535865b7

Parent:

2:9847e6e7ce2d

Child:

4:851261a9273b

--- a/main.cpp	Mon Jul 05 17:30:13 2021 +0000
+++ b/main.cpp	Mon Jul 05 19:50:56 2021 +0000
@@ -15,6 +15,14 @@
 Servo gobeur_left_block_servo(servo_block_1_pin);
 Servo gobeur_right_block_servo(servo_block_2_pin);
  
+ros::NodeHandle  nh;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+ros::Subscriber<gobeur_node::Gobeur> sub("gobeur", &gobeur_action);
+
+char hello[13] = "hello!";
 
 void gobeur_initialize()
 {
@@ -26,45 +34,67 @@
     wait(2);
     gobeur_right_aspire_servo = 0;
     gobeur_left_aspire_servo = 0;
+    wait(2);
+    nh.loginfo("Actuators initialised");
     }
     
 int aspirateur_on(uint8_t gobeur_id) 
 {
     if(gobeur_id == 0)
-        gobeur_right_aspire_servo = 0.2;
+    {
+        gobeur_right_aspire_servo = 0.15;
+        nh.loginfo("Aspirateur right on");
+        }
+
     if(gobeur_id == 1)
-        gobeur_left_aspire_servo = 0.2;
+    {
+        gobeur_left_aspire_servo = 0.15;
+        nh.loginfo("Aspirateur left on");
+        }
 }
 
 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);
-        }*/
+    {
+        gobeur_right_aspire_servo = 0.0;
+        nh.loginfo("Aspirateur right off");
+        }
+
     if(gobeur_id == 1)
-        gobeur_left_aspire_servo  = 0;
+    {
+        gobeur_left_aspire_servo = 0.0;
+        nh.loginfo("Aspirateur left off");
+        }
 }
 
 int block_gobelet(uint8_t gobeur_id) 
 {
     if(gobeur_id == 0)
-        gobeur_right_block_servo = 0.2;
+        {
+        gobeur_right_block_servo = 0.15;
+        nh.loginfo("Gobelet right blocked");
+        }
     if(gobeur_id == 1)
-        gobeur_left_block_servo = 0.2;
+        {
+        gobeur_left_block_servo = 0.15;
+        nh.loginfo("Gobelet left blocked");
+        }
 }
 int unblock_gobelet(uint8_t gobeur_id)
 {
-    if(gobeur_id == 0)
-        gobeur_right_block_servo = 0;
+        {
+        gobeur_right_block_servo = 0.0;
+        nh.loginfo("Gobelet right unblocked");
+        }
     if(gobeur_id == 1)
-        gobeur_left_block_servo = 0;
+        {
+        gobeur_left_block_servo = 0.0;
+        nh.loginfo("Gobelet left unblocked");
+        }
 }
 
-void gobeur_action(const stm32_gobeur::Gobeur& gobeur_msg){
+void gobeur_action(const gobeur_node::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 ; 
@@ -76,42 +106,38 @@
     }
 }
 
-    
-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);
-
+    nh.subscribe(sub);
+    
+        str_msg.data = hello;
+        chatter.publish( &str_msg );
+        
     while (1) {
-        led = !led;
         
         switch(gobeur_right_aspire) {
-            case 2 : aspirateur_on(GOBEUR_RIGHT);
-                     unblock_gobelet(GOBEUR_RIGHT);
-                     wait_ms(2000);
+            case 2 : unblock_gobelet(GOBEUR_RIGHT); 
+                     aspirateur_on(GOBEUR_RIGHT);
+                     wait(2);
                      block_gobelet(GOBEUR_RIGHT);
                      aspirateur_off(GOBEUR_RIGHT);
+                     gobeur_right_aspire = 0;
                      break;      
             case 1 : aspirateur_off(GOBEUR_RIGHT);
                      unblock_gobelet(GOBEUR_RIGHT); 
-                     wait_ms(2000);
+                     gobeur_right_aspire = 0;
                      break;      
             case 0 : break;
             }
-        
-        str_msg.data = hello;
-        chatter.publish( &str_msg );
+            
+        switch(gobeur_right_status) {
+            case 2: aspirateur_on(GOBEUR_RIGHT);
+                     break;      
+            case 1 : aspirateur_off(GOBEUR_RIGHT);
+                     break;      
+            }
+            gobeur_right_status = -1;
         nh.spinOnce();
-        wait_ms(1000);
     }
 }
\ No newline at end of file