Diff: main.cpp

Revision:

6:4af07c2de6ba

Parent:

5:c38735ab64dc

Child:

9:59a9831f067d

--- a/main.cpp	Tue Jul 06 11:59:27 2021 +0000
+++ b/main.cpp	Thu Jul 08 14:11:48 2021 +0000
@@ -7,140 +7,69 @@
 #include <ros.h>
 #include <std_msgs/String.h>
 #include <Gobeur.h>
-#include <gobeur_lib.h>
+#include <Manche_a_air.h>
 #include <Servo.h>
-
-Servo gobeur_right_aspire_servo(servo_aspire_1_pin);
-Servo gobeur_left_aspire_servo(servo_aspire_2_pin);
-Servo gobeur_right_block_servo(servo_block_1_pin);
-Servo gobeur_left_block_servo(servo_block_2_pin);
+#include "actuators.h"
+ 
+Gobeur* gobeurs[2];
+Manche_a_air* maas[3]; 
+ 
  
 ros::NodeHandle  nh;
 
 std_msgs::String str_msg;
 ros::Publisher chatter("chatter", &str_msg);
 
-ros::Subscriber<gobeur_node::Gobeur> sub("gobeur", &gobeur_action);
+ros::Subscriber<gobeur_node::Gobeur_msg> sub_gobeur("gobeur", &gobeur_trait_msg);
+ros::Subscriber<manche_a_air_node::Manche_a_air_msg> sub_maa("manche_a_air", &maa_trait_msg);
 
 char hello[13] = "hello!";
 
-void gobeur_initialize()
-{
-    gobeur_right_block_servo = 0.0;
-    gobeur_left_block_servo = 0.0
-    
-    gobeur_right_aspire_servo = 0.0;
-    gobeur_left_aspire_servo = 0.0;
-    wait(2);
-    gobeur_right_aspire_servo = 1.0;
-    gobeur_left_aspire_servo = 1.0;
-    wait(2);
-    gobeur_right_aspire_servo = 0.0;
-    gobeur_left_aspire_servo = 0.0;
-    wait(2);
-    nh.loginfo("Actuators initialised");
-    }
-    
-int aspirateur_on(uint8_t gobeur_id) 
-{
-    if(gobeur_id == 0)
-    {
-        gobeur_right_aspire_servo = 0.15;
-        nh.loginfo("Aspirateur right on");
-        }
 
-    if(gobeur_id == 1)
-    {
-        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.0;
-        nh.loginfo("Aspirateur right off");
-        }
+float clock_s() { return us_ticker_read() / 1000000.0f; }
+uint64_t clock_ms() { return us_ticker_read() / 1000; }
+uint64_t clock_us() { return us_ticker_read(); }
 
-    if(gobeur_id == 1)
-    {
-        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.15;
-        nh.loginfo("Gobelet right blocked");
-        }
-    if(gobeur_id == 1)
-        {
-        gobeur_left_block_servo = 0.15;
-        nh.loginfo("Gobelet left blocked");
-        }
-}
-int unblock_gobelet(uint8_t gobeur_id)
+void gobeur_trait_msg(const gobeur_node::Gobeur_msg& gobeur_msg)
 {
-        {
-        gobeur_right_block_servo = 0.0;
-        nh.loginfo("Gobelet right unblocked");
-        }
-    if(gobeur_id == 1)
-        {
-        gobeur_left_block_servo = 0.0;
-        nh.loginfo("Gobelet left unblocked");
-        }
+    gobeurs[gobeur_msg.index_gobeur]->update_order(gobeur_msg.gobeur_order) ; 
 }
-
-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 ; 
-    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(); 
-    }
+void maa_trait_msg(const manche_a_air_node::Manche_a_air_msg& maa_msg)
+{
+    maas[maa_msg.index_manche_a_air]->update_order(maa_msg.manche_a_air_order) ; 
 }
 
 int main() {
+    gobeurs[0] = new Gobeur(GOBEUR_TURBINE_LEFT_PIN, GOBEUR_SERVO_LEFT_PIN, GOBEUR_TURBINE_TH_ON, GOBEUR_TURBINE_TH_OFF, GOBEUR_SERVO_POS_OPEN, GOBEUR_SERVO_POS_CLOSE );
+    gobeurs[1] = new Gobeur(GOBEUR_TURBINE_RIGHT_PIN, GOBEUR_SERVO_RIGHT_PIN, GOBEUR_TURBINE_TH_ON, GOBEUR_TURBINE_TH_OFF, GOBEUR_SERVO_POS_OPEN, GOBEUR_SERVO_POS_CLOSE );
+    maas[0] = new Manche_a_air(MAA_SERVO_LEFT_PIN, MAA_SERVO_POS_OPEN, MAA_SERVO_POS_CLOSE);
+    maas[1] = new Manche_a_air(MAA_SERVO_RIGHT_PIN, MAA_SERVO_POS_OPEN, MAA_SERVO_POS_CLOSE);
+    maas[2] = new Manche_a_air(FLAG_SERVO_PIN, FLAG_SERVO_POS_OPEN, FLAG_SERVO_POS_CLOSE);
+    
+    //maas[2] = new Manche_a_air(FLAG_SERVO_PIN, FLAG_SERVO_POS_OPEN, FLAG_SERVO_POS_CLOSE);
+    maas[2]->set_auto_open(1, 1);
+    maas[2]->set_auto_close(1, 5);
+    maas[0]->set_auto_open(1, 2);
+    maas[0]->set_auto_close(1, 4);
+    maas[1]->set_auto_open(1, 3);
+    maas[1]->set_auto_close(1, 3);
+    
     nh.initNode();
     nh.advertise(chatter);
-    nh.subscribe(sub);
+    nh.subscribe(sub_gobeur);
+    nh.subscribe(sub_maa);
     
-        str_msg.data = hello;
-        chatter.publish( &str_msg );
+    str_msg.data = hello;
+    chatter.publish( &str_msg );
         
     while (1) {
+        gobeurs[0]->automate();
+        gobeurs[1]->automate();
+        maas[0]->automate();
+        maas[1]->automate();
+        maas[2]->automate();
+        nh.spinOnce();
         
-        switch(gobeur_right_aspire) {
-            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); 
-                     gobeur_right_aspire = 0;
-                     break;      
-            case 0 : break;
-            }
-            
-        switch(gobeur_right_status) {
-            case 2: aspirateur_on(GOBEUR_RIGHT);
-                     break;      
-            case 1 : aspirateur_off(GOBEUR_RIGHT);
-                     break;      
-            }
-            gobeur_right_status = -1;
-        nh.spinOnce();
     }
-}
\ No newline at end of file
+}
+  
\ No newline at end of file