From 4e1c9ec5cec61ac5153b736690cc99e63c151418 Mon Sep 17 00:00:00 2001
From: thomas <thomas.vadebout@mailo.com>
Date: Wed, 23 Jun 2021 01:26:18 +0200
Subject: [PATCH] add alim Debug and Arm

---
 src/main.cpp | 104 +++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 96 insertions(+), 8 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 2daf31b..d5ef0f3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,7 +1,8 @@
+// TODO : créer un message pour les debugs de la carte d'alimentation (4 float)
+
 #include <LiquidCrystal_I2C.h>
 
 #include <Servo.h>
-#include <Stepper.h>
 
 #include <ros.h>
 #include <std_msgs/Bool.h>
@@ -9,17 +10,47 @@
 
 // Servo object declaration
 Servo flagServo; 
+#define FLAG_UNFOLDED_ANGLE 170
+#define FLAG_FOLDED_ANGLE 0
+
+Servo armServo; //pour manche à air
+#define ARM_UNFOLDED_ANGLE 90
+#define ARM_FOLDED_ANGLE 0
 
 // Pin declaration
-const int flagServoPin = D5;
-const int pullCordPin = D8;
-const int switchPin = D9;
+const int flagServoPin = D6;
+const int armServoPin = D4;
+
+const int pullCordPin = A6;
+const int switchPin = A7;
 const int buttonPin = D10;
-const int buttonLedPin = D11;
+const int buttonLedPin = D12;
+
+const int voltage_twelvePin = A3;
+const int voltage_fivePin = A0;
+const int current_twelvePin = A2;
+const int current_fivePin = A1;
+#define VOLT12_RATIO 1
+#define VOLT5_RATIO 1
+#define CURR5_RATIO 1
+#define CURR12_RATIO 1
+
+const int bauStatusPin = D8;
+const int bauSoftPin = D9;
+
 
 // Flag state
 bool flagStatus;
 
+// BAU state
+bool bauSoftStatus;
+bool bauStatus;
+
+// Arm state
+bool armStatus;
+
+
+
 // Current team (either TEAM_YELLOW or TEAM_BLUE)
 bool team;
 bool buttonStatus;
@@ -45,10 +76,13 @@ ros::NodeHandle nh;
 std_msgs::Bool teamMsg;
 std_msgs::Bool buttonStatusMsg;
 std_msgs::Bool pullCordStatusMsg;
+std_msgs::Bool bauStatusMsg;
 
 ros::Publisher teamStatusPub("team", &teamMsg);
 ros::Publisher buttonStatusPub("button_status", &buttonStatusMsg);
 ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg);
+ros::Publisher bauStatusPub("BAU_status", &bauStatusMsg);
+ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg);
 
 void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore);
 ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimatedScoreCallback);
@@ -56,6 +90,11 @@ ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimated
 void flagStatusChange(const std_msgs::Bool &flagStatusMsg);
 ros::Subscriber<std_msgs::Bool> flagStatusSub("flag_state", &flagStatusChange);
 
+void armStatusChange(const std_msgs::Bool &armStatusMsg);
+ros::Subscriber<std_msgs::Bool> flagStatusSub("flag_state", &armStatusChange);
+
+void bauSoftSet(const std_msgs::Bool &bauSoftSetMsg);
+ros::Subscriber<std_msgs::Bool> flagStatusSub("flag_state", &bauSoftSet);
 
 // LCD screen
 LiquidCrystal_I2C lcd(0x27, 16, 2);
@@ -101,6 +140,8 @@ void setup()
   nh.advertise(teamStatusPub);
   nh.advertise(buttonStatusPub);
   nh.advertise(pullCordStatusPub);
+  nh.advertise(bauStatusPub);
+
   nh.subscribe(estimatedScoreSub);
   nh.subscribe(flagStatusSub);
 
@@ -124,12 +165,42 @@ 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)
+// Appelé quand l'état du drapeau est mis à jour
 void flagStatusChange(const std_msgs::Bool &flagStatusMsg)
 {
   flagStatus = flagStatusMsg.data;
 }
 
+// Appelé quand l'état du BAU est mis à jour
+void bauSoftSet(const std_msgs::Bool &bauSoftSetMsg)
+{
+    bauSoftStatus = bauSoftSetMsg.data;
+}
+
+// Appelé quand l'état du bras (manche à air) est mis à jour
+void armStatusChange(const std_msgs::Bool &armStatusMsg)
+{
+    armStatus = armStatusMsg.data;
+}
+
+
+
+void manage_supply_card_data(){
+
+  int volt12_valueADC = analogRead(voltage_twelvePin);
+  int volt5_valueADC = analogRead(voltage_fivePin);
+  int curr5_valueADC = analogRead(current_fivePin);
+  int curr12_valueADC = analogRead(current_twelvePin);
+
+  float volt12_value = (float) volt12_valueADC * VOLT12_RATIO;
+  float volt5_value = (float) volt5_valueADC * VOLT5_RATIO;
+  float curr5_value = (float) curr5_valueADC * CURR5_RATIO;
+  float curr12_value = (float) curr12_valueADC * CURR12_RATIO;
+
+
+}
+
+
 void loop()
 {
 
@@ -190,6 +261,8 @@ void loop()
     refreshScreen();
   }
 
+  bauStatus = digitalRead(bauStatusPin);
+
   if (millis() - lastMessageTime >= MSG_SENDING_INTERVAL)
   {
     teamMsg.data = team;
@@ -201,15 +274,30 @@ void loop()
     buttonStatusMsg.data = buttonStatus;
     buttonStatusPub.publish(&buttonStatusMsg);
 
+    bauStatusMsg.data = bauStatus;
+    bauStatusPub.publish(&bauStatusMsg);
+
+
     lastMessageTime = millis();
     nh.spinOnce();
   }
 
   if(flagStatus)
   {
-    flagServo.write(170);
+    flagServo.write(FLAG_UNFOLDED_ANGLE);
   } else {
-    flagServo.write(0);
+    flagServo.write(FLAG_FOLDED_ANGLE);
   }
+
+  if(armStatus)
+  {
+    armServo.write(ARM_UNFOLDED_ANGLE);
+  } else {
+    armServo.write(ARM_FOLDED_ANGLE);
+  }
+
+  digitalWrite(bauSoftPin, bauSoftStatus);
+
+
   
 }
\ No newline at end of file
-- 
GitLab