![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
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