diff --git a/src/main.cpp b/src/main.cpp index 63dd2c69970e81fd514a58fe9bd7b96a8e20de6e..2daf31b345bd04c3d9808142eb12164d61340afd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,15 +1,25 @@ #include <LiquidCrystal_I2C.h> +#include <Servo.h> +#include <Stepper.h> + #include <ros.h> #include <std_msgs/Bool.h> #include <std_msgs/Int32.h> +// Servo object declaration +Servo flagServo; + // Pin declaration +const int flagServoPin = D5; const int pullCordPin = D8; const int switchPin = D9; const int buttonPin = D10; const int buttonLedPin = D11; +// Flag state +bool flagStatus; + // Current team (either TEAM_YELLOW or TEAM_BLUE) bool team; bool buttonStatus; @@ -43,6 +53,10 @@ ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg); void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore); ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimatedScoreCallback); +void flagStatusChange(const std_msgs::Bool &flagStatusMsg); +ros::Subscriber<std_msgs::Bool> flagStatusSub("flag_state", &flagStatusChange); + + // LCD screen LiquidCrystal_I2C lcd(0x27, 16, 2); @@ -63,8 +77,13 @@ void refreshScreen() lcd.print(buttonStatus ? "ON" : "OFF"); } + + void setup() { + flagServo.attach(flagServoPin); + flagServo.write(0); + pinMode(pullCordPin, INPUT_PULLUP); pinMode(switchPin, INPUT_PULLUP); pinMode(buttonPin, INPUT_PULLUP); @@ -83,6 +102,7 @@ void setup() nh.advertise(buttonStatusPub); nh.advertise(pullCordStatusPub); nh.subscribe(estimatedScoreSub); + nh.subscribe(flagStatusSub); lcd.clear(); lcd.setCursor(0, 0); @@ -104,6 +124,12 @@ void estimatedScoreCallback(const std_msgs::Int32 &estimatedScoreMsg) refreshScreen(); } +// Appelé quand l'état du drapeau est mis à jour (message de score reçu sur l'Arduino depuis la raspi) +void flagStatusChange(const std_msgs::Bool &flagStatusMsg) +{ + flagStatus = flagStatusMsg.data; +} + void loop() { @@ -178,4 +204,12 @@ void loop() lastMessageTime = millis(); nh.spinOnce(); } + + if(flagStatus) + { + flagServo.write(170); + } else { + flagServo.write(0); + } + } \ No newline at end of file