![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
action gobeur
Dependencies: ros_messages mbed Servo Manche_a_air Gobeur ros_lib_melodic
main.cpp@11:9038da3b84da, 2021-07-08 (annotated)
- Committer:
- kyxstark
- Date:
- Thu Jul 08 22:48:21 2021 +0000
- Revision:
- 11:9038da3b84da
- Parent:
- 9:59a9831f067d
pinout, pos servo;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mondiaye | 1:d6bbefff36fa | 1 | /* |
mondiaye | 1:d6bbefff36fa | 2 | * rosserial Publisher Example |
mondiaye | 1:d6bbefff36fa | 3 | * Prints "hello world!" |
mondiaye | 1:d6bbefff36fa | 4 | */ |
mondiaye | 1:d6bbefff36fa | 5 | |
mondiaye | 1:d6bbefff36fa | 6 | #include"mbed.h" |
mondiaye | 1:d6bbefff36fa | 7 | #include <ros.h> |
mondiaye | 1:d6bbefff36fa | 8 | #include <std_msgs/String.h> |
mondiaye | 1:d6bbefff36fa | 9 | #include <Gobeur.h> |
kyxstark | 6:4af07c2de6ba | 10 | #include <Manche_a_air.h> |
mondiaye | 1:d6bbefff36fa | 11 | #include <Servo.h> |
kyxstark | 9:59a9831f067d | 12 | #include <ros_messages/game_manager/GameStatus.h> |
kyxstark | 6:4af07c2de6ba | 13 | #include "actuators.h" |
kyxstark | 6:4af07c2de6ba | 14 | |
kyxstark | 6:4af07c2de6ba | 15 | Gobeur* gobeurs[2]; |
kyxstark | 6:4af07c2de6ba | 16 | Manche_a_air* maas[3]; |
kyxstark | 9:59a9831f067d | 17 | |
kyxstark | 9:59a9831f067d | 18 | int game_started_f = 0; |
kyxstark | 9:59a9831f067d | 19 | |
kyxstark | 9:59a9831f067d | 20 | |
kyxstark | 11:9038da3b84da | 21 | void gobeur_trait_msg(const stm32_actuators_node::Gobeur_msg& gobeur_msg) |
kyxstark | 9:59a9831f067d | 22 | { |
kyxstark | 9:59a9831f067d | 23 | gobeurs[gobeur_msg.index_gobeur]->update_order(gobeur_msg.gobeur_order) ; |
kyxstark | 9:59a9831f067d | 24 | } |
kyxstark | 11:9038da3b84da | 25 | void maa_trait_msg(const stm32_actuators_node::Manche_a_air_msg& maa_msg) |
kyxstark | 9:59a9831f067d | 26 | { |
kyxstark | 9:59a9831f067d | 27 | maas[maa_msg.index_manche_a_air]->update_order(maa_msg.manche_a_air_order) ; |
kyxstark | 9:59a9831f067d | 28 | } |
kyxstark | 9:59a9831f067d | 29 | void on_game_status(const game_manager_node::game_status_msg& game_status) |
kyxstark | 9:59a9831f067d | 30 | { |
kyxstark | 11:9038da3b84da | 31 | if(game_started_f != 1 && game_status.game_status == 1) |
kyxstark | 11:9038da3b84da | 32 | { |
kyxstark | 11:9038da3b84da | 33 | game_started_f = 1; |
kyxstark | 11:9038da3b84da | 34 | maas[2]->set_auto_open(1, 95); |
kyxstark | 11:9038da3b84da | 35 | } |
kyxstark | 11:9038da3b84da | 36 | else if(game_status.game_status == 0) |
kyxstark | 11:9038da3b84da | 37 | { |
kyxstark | 11:9038da3b84da | 38 | } |
kyxstark | 11:9038da3b84da | 39 | else if(game_status.game_status == 2) |
kyxstark | 11:9038da3b84da | 40 | { |
kyxstark | 11:9038da3b84da | 41 | } |
kyxstark | 11:9038da3b84da | 42 | |
kyxstark | 9:59a9831f067d | 43 | } |
kyxstark | 9:59a9831f067d | 44 | |
kyxstark | 6:4af07c2de6ba | 45 | |
mondiaye | 1:d6bbefff36fa | 46 | |
mondiaye | 3:a284535865b7 | 47 | ros::NodeHandle nh; |
mondiaye | 3:a284535865b7 | 48 | |
mondiaye | 3:a284535865b7 | 49 | std_msgs::String str_msg; |
mondiaye | 3:a284535865b7 | 50 | ros::Publisher chatter("chatter", &str_msg); |
mondiaye | 3:a284535865b7 | 51 | |
kyxstark | 11:9038da3b84da | 52 | ros::Subscriber<stm32_actuators_node::Gobeur_msg> sub_gobeur("gobeur", &gobeur_trait_msg); |
kyxstark | 11:9038da3b84da | 53 | ros::Subscriber<stm32_actuators_node::Manche_a_air_msg> sub_maa("manche_a_air", &maa_trait_msg); |
mondiaye | 3:a284535865b7 | 54 | |
kyxstark | 9:59a9831f067d | 55 | |
kyxstark | 9:59a9831f067d | 56 | ros::Subscriber<game_manager_node::game_status_msg> sub_game_status ("ai/game_manager/status", &on_game_status) ; |
kyxstark | 9:59a9831f067d | 57 | |
mondiaye | 3:a284535865b7 | 58 | char hello[13] = "hello!"; |
mondiaye | 1:d6bbefff36fa | 59 | |
mondiaye | 3:a284535865b7 | 60 | |
kyxstark | 6:4af07c2de6ba | 61 | float clock_s() { return us_ticker_read() / 1000000.0f; } |
kyxstark | 6:4af07c2de6ba | 62 | uint64_t clock_ms() { return us_ticker_read() / 1000; } |
kyxstark | 6:4af07c2de6ba | 63 | uint64_t clock_us() { return us_ticker_read(); } |
mondiaye | 3:a284535865b7 | 64 | |
mondiaye | 1:d6bbefff36fa | 65 | |
mondiaye | 1:d6bbefff36fa | 66 | int main() { |
kyxstark | 11:9038da3b84da | 67 | 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 ); |
kyxstark | 6:4af07c2de6ba | 68 | 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 ); |
kyxstark | 11:9038da3b84da | 69 | maas[0] = new Manche_a_air(MAA_SERVO_LEFT_PIN, MAA_SERVO_POS_CLOSE, MAA_SERVO_POS_OPEN); |
kyxstark | 6:4af07c2de6ba | 70 | maas[1] = new Manche_a_air(MAA_SERVO_RIGHT_PIN, MAA_SERVO_POS_OPEN, MAA_SERVO_POS_CLOSE); |
kyxstark | 6:4af07c2de6ba | 71 | maas[2] = new Manche_a_air(FLAG_SERVO_PIN, FLAG_SERVO_POS_OPEN, FLAG_SERVO_POS_CLOSE); |
kyxstark | 6:4af07c2de6ba | 72 | |
kyxstark | 6:4af07c2de6ba | 73 | //maas[2] = new Manche_a_air(FLAG_SERVO_PIN, FLAG_SERVO_POS_OPEN, FLAG_SERVO_POS_CLOSE); |
kyxstark | 11:9038da3b84da | 74 | //maas[2]->set_auto_open(1, 1); |
kyxstark | 11:9038da3b84da | 75 | //maas[2]->set_auto_close(1, 5); |
kyxstark | 11:9038da3b84da | 76 | //maas[0]->set_auto_open(1, 2); |
kyxstark | 11:9038da3b84da | 77 | //maas[0]->set_auto_close(1, 4); |
kyxstark | 11:9038da3b84da | 78 | //maas[1]->set_auto_open(1, 3); |
kyxstark | 11:9038da3b84da | 79 | //maas[1]->set_auto_close(1, 3); |
kyxstark | 6:4af07c2de6ba | 80 | |
mondiaye | 1:d6bbefff36fa | 81 | nh.initNode(); |
mondiaye | 1:d6bbefff36fa | 82 | nh.advertise(chatter); |
kyxstark | 6:4af07c2de6ba | 83 | nh.subscribe(sub_gobeur); |
kyxstark | 6:4af07c2de6ba | 84 | nh.subscribe(sub_maa); |
kyxstark | 9:59a9831f067d | 85 | nh.subscribe(sub_game_status); |
mondiaye | 3:a284535865b7 | 86 | |
kyxstark | 6:4af07c2de6ba | 87 | str_msg.data = hello; |
kyxstark | 6:4af07c2de6ba | 88 | chatter.publish( &str_msg ); |
mondiaye | 3:a284535865b7 | 89 | |
mondiaye | 1:d6bbefff36fa | 90 | while (1) { |
kyxstark | 6:4af07c2de6ba | 91 | gobeurs[0]->automate(); |
kyxstark | 6:4af07c2de6ba | 92 | gobeurs[1]->automate(); |
kyxstark | 6:4af07c2de6ba | 93 | maas[0]->automate(); |
kyxstark | 6:4af07c2de6ba | 94 | maas[1]->automate(); |
kyxstark | 6:4af07c2de6ba | 95 | maas[2]->automate(); |
kyxstark | 6:4af07c2de6ba | 96 | nh.spinOnce(); |
mondiaye | 1:d6bbefff36fa | 97 | |
mondiaye | 1:d6bbefff36fa | 98 | } |
kyxstark | 6:4af07c2de6ba | 99 | } |
kyxstark | 6:4af07c2de6ba | 100 |