Skip to content
Snippets Groups Projects

Replace main.cpp

Merged Imported Clubelek Asso requested to merge marc.le-gal-la-salle-master-patch-29656 into master
1 file
+ 34
0
Compare changes
  • Side-by-side
  • Inline
+ 34
0
#include <LiquidCrystal_I2C.h>
#include <LiquidCrystal_I2C.h>
 
#include <Servo.h>
 
#include <Stepper.h>
 
#include <ros.h>
#include <ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int32.h>
 
// Servo object declaration
 
Servo flagServo;
 
// Pin declaration
// Pin declaration
 
const int flagServoPin = D5;
const int pullCordPin = D8;
const int pullCordPin = D8;
const int switchPin = D9;
const int switchPin = D9;
const int buttonPin = D10;
const int buttonPin = D10;
const int buttonLedPin = D11;
const int buttonLedPin = D11;
 
// Flag state
 
bool flagStatus;
 
// Current team (either TEAM_YELLOW or TEAM_BLUE)
// Current team (either TEAM_YELLOW or TEAM_BLUE)
bool team;
bool team;
bool buttonStatus;
bool buttonStatus;
@@ -43,6 +53,10 @@ ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg);
@@ -43,6 +53,10 @@ ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg);
void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore);
void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore);
ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimatedScoreCallback);
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
// LCD screen
LiquidCrystal_I2C lcd(0x27, 16, 2);
LiquidCrystal_I2C lcd(0x27, 16, 2);
@@ -63,8 +77,13 @@ void refreshScreen()
@@ -63,8 +77,13 @@ void refreshScreen()
lcd.print(buttonStatus ? "ON" : "OFF");
lcd.print(buttonStatus ? "ON" : "OFF");
}
}
 
 
void setup()
void setup()
{
{
 
flagServo.attach(flagServoPin);
 
flagServo.write(0);
 
pinMode(pullCordPin, INPUT_PULLUP);
pinMode(pullCordPin, INPUT_PULLUP);
pinMode(switchPin, INPUT_PULLUP);
pinMode(switchPin, INPUT_PULLUP);
pinMode(buttonPin, INPUT_PULLUP);
pinMode(buttonPin, INPUT_PULLUP);
@@ -83,6 +102,7 @@ void setup()
@@ -83,6 +102,7 @@ void setup()
nh.advertise(buttonStatusPub);
nh.advertise(buttonStatusPub);
nh.advertise(pullCordStatusPub);
nh.advertise(pullCordStatusPub);
nh.subscribe(estimatedScoreSub);
nh.subscribe(estimatedScoreSub);
 
nh.subscribe(flagStatusSub);
lcd.clear();
lcd.clear();
lcd.setCursor(0, 0);
lcd.setCursor(0, 0);
@@ -104,6 +124,12 @@ void estimatedScoreCallback(const std_msgs::Int32 &estimatedScoreMsg)
@@ -104,6 +124,12 @@ void estimatedScoreCallback(const std_msgs::Int32 &estimatedScoreMsg)
refreshScreen();
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()
void loop()
{
{
@@ -178,4 +204,12 @@ void loop()
@@ -178,4 +204,12 @@ void loop()
lastMessageTime = millis();
lastMessageTime = millis();
nh.spinOnce();
nh.spinOnce();
}
}
 
 
if(flagStatus)
 
{
 
flagServo.write(170);
 
} else {
 
flagServo.write(0);
 
}
 
}
}
 
\ No newline at end of file