action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
Revision 11:9038da3b84da, committed 2021-07-08
- Comitter:
- kyxstark
- Date:
- Thu Jul 08 22:48:21 2021 +0000
- Parent:
- 10:3054cad9c21b
- Commit message:
- pinout, pos servo;
Changed in this revision
--- a/Gobeur.lib Thu Jul 08 17:16:00 2021 +0000 +++ b/Gobeur.lib Thu Jul 08 22:48:21 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/UTCoupe/code/Gobeur/#bc5e3a8e3f92 +https://os.mbed.com/teams/UTCoupe/code/Gobeur/#64a88f154e65
--- a/Manche_a_air.lib Thu Jul 08 17:16:00 2021 +0000 +++ b/Manche_a_air.lib Thu Jul 08 22:48:21 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/kyxstark/code/Manche_a_air/#de7ca80dbe4b +https://os.mbed.com/users/kyxstark/code/Manche_a_air/#04d111f63a0f
--- a/actuators.h Thu Jul 08 17:16:00 2021 +0000 +++ b/actuators.h Thu Jul 08 22:48:21 2021 +0000 @@ -5,18 +5,18 @@ #define GOBEUR_TURBINE_LEFT_PIN PB_6 // PWM 2 14/3 #define GOBEUR_SERVO_LEFT_PIN PA_9 // PWM 0 1/2 -#define GOBEUR_TURBINE_RIGHT_PIN PB_7 // PWM 3 14/2 +#define GOBEUR_TURBINE_RIGHT_PIN PB_2 // PWM 3 14/2 //#define GOBEUR_SERVO_RIGHT_PIN PA_15 // PWM 1 2/1 -#define GOBEUR_SERVO_RIGHT_PIN PB_2 // PWM 1 2/4 +#define GOBEUR_SERVO_RIGHT_PIN PB_7 // PWM 7 2/4 #define GOBEUR_SERVO_POS_OPEN 1 #define GOBEUR_SERVO_POS_CLOSE 0 -#define GOBEUR_TURBINE_TH_ON 1 +#define GOBEUR_TURBINE_TH_ON 0 #define GOBEUR_TURBINE_TH_OFF 0 -#define MAA_SERVO_LEFT_PIN PA_0 // PWM 4 2/1 -#define MAA_SERVO_RIGHT_PIN PA_1 // PWM 5 2/2 +#define MAA_SERVO_LEFT_PIN PA_1 // PWM 4 2/1 +#define MAA_SERVO_RIGHT_PIN PA_0 // PWM 5 2/2 #define MAA_SERVO_POS_OPEN 1 @@ -24,8 +24,8 @@ #define FLAG_SERVO_PIN PA_6 // PWM 6 3/1 -#define FLAG_SERVO_POS_OPEN 1 -#define FLAG_SERVO_POS_CLOSE 0 +#define FLAG_SERVO_POS_OPEN 0.95 +#define FLAG_SERVO_POS_CLOSE 0.96//0.11 float clock_s();
--- a/main.cpp Thu Jul 08 17:16:00 2021 +0000 +++ b/main.cpp Thu Jul 08 22:48:21 2021 +0000 @@ -18,18 +18,28 @@ int game_started_f = 0; -void gobeur_trait_msg(const gobeur_node::Gobeur_msg& gobeur_msg) +void gobeur_trait_msg(const stm32_actuators_node::Gobeur_msg& gobeur_msg) { gobeurs[gobeur_msg.index_gobeur]->update_order(gobeur_msg.gobeur_order) ; } -void maa_trait_msg(const manche_a_air_node::Manche_a_air_msg& maa_msg) +void maa_trait_msg(const stm32_actuators_node::Manche_a_air_msg& maa_msg) { maas[maa_msg.index_manche_a_air]->update_order(maa_msg.manche_a_air_order) ; } void on_game_status(const game_manager_node::game_status_msg& game_status) { - game_started_f = 1; - maas[2]->set_auto_open(1, 97); + if(game_started_f != 1 && game_status.game_status == 1) + { + game_started_f = 1; + maas[2]->set_auto_open(1, 95); + } + else if(game_status.game_status == 0) + { + } + else if(game_status.game_status == 2) + { + } + } @@ -39,8 +49,8 @@ std_msgs::String str_msg; ros::Publisher chatter("chatter", &str_msg); -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); +ros::Subscriber<stm32_actuators_node::Gobeur_msg> sub_gobeur("gobeur", &gobeur_trait_msg); +ros::Subscriber<stm32_actuators_node::Manche_a_air_msg> sub_maa("manche_a_air", &maa_trait_msg); ros::Subscriber<game_manager_node::game_status_msg> sub_game_status ("ai/game_manager/status", &on_game_status) ; @@ -54,19 +64,19 @@ 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[0] = new Gobeur(GOBEUR_TURBINE_LEFT_PIN, GOBEUR_SERVO_LEFT_PIN, GOBEUR_TURBINE_TH_ON, GOBEUR_TURBINE_TH_OFF, GOBEUR_SERVO_POS_CLOSE, GOBEUR_SERVO_POS_OPEN ); 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[0] = new Manche_a_air(MAA_SERVO_LEFT_PIN, MAA_SERVO_POS_CLOSE, MAA_SERVO_POS_OPEN); 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); + //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);
--- a/ros_messages.lib Thu Jul 08 17:16:00 2021 +0000 +++ b/ros_messages.lib Thu Jul 08 22:48:21 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/UTCoupe/code/ros_messages/#49ecb29f2afc +https://os.mbed.com/teams/UTCoupe/code/ros_messages/#60f7c46cc24c