diff --git a/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp b/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp index 79f43894fefa3b7c0b3afe4530497baddebc23d8..87b1b09e0efab56abc76757e5e5848832e14dd94 100644 --- a/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp +++ b/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp @@ -58,7 +58,6 @@ void LiquidCrystal_I2C::init(){ void LiquidCrystal_I2C::init_priv() { - Wire.begin(); _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS; begin(_cols, _rows); } diff --git a/src/main.cpp b/src/main.cpp index d4a7f3e4b2ce733328634e9b32680f56938a85c7..8d0a17c31464b79f397eaabe75965682603370dc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,6 +7,7 @@ #include <ros.h> #include <std_msgs/Bool.h> #include <std_msgs/Int32.h> +#include <ctrlpan_msgs/alim_stat.h> // Servo object declaration Servo flagServo; @@ -17,6 +18,11 @@ Servo armServo; //pour manche à air #define ARM_UNFOLDED_ANGLE 90 #define ARM_FOLDED_ANGLE 0 +// I2C : (if not working : try inverse sda and scl pin) +// todo : remove nucleo jumpers to use these pins. +#define I2C_SDA A5 +#define I2C_SCL A4 + // Pin declaration const int flagServoPin = D6; const int armServoPin = D4; @@ -77,11 +83,13 @@ std_msgs::Bool teamMsg; std_msgs::Bool buttonStatusMsg; std_msgs::Bool pullCordStatusMsg; std_msgs::Bool bauStatusMsg; +ctrlpan_msgs::alim_stat alimStatMsg; ros::Publisher teamStatusPub("team", &teamMsg); ros::Publisher buttonStatusPub("button_status", &buttonStatusMsg); ros::Publisher bauStatusPub("BAU_status", &bauStatusMsg); ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg); +ros::Publisher alimStatPub("alim_stat", &alimStatMsg); void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore); ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimatedScoreCallback); @@ -119,9 +127,17 @@ void refreshScreen() void setup() { + + // INIT Wire on : A4 => SDA || A5 => SCL + Wire.begin(I2C_SDA, I2C_SCL); + + flagServo.attach(flagServoPin); flagServo.write(0); + armServo.attach(armServoPin); + armServo.write(0); + pinMode(pullCordPin, INPUT_PULLUP); pinMode(switchPin, INPUT_PULLUP); pinMode(buttonPin, INPUT_PULLUP); @@ -143,9 +159,11 @@ void setup() nh.advertise(buttonStatusPub); nh.advertise(pullCordStatusPub); nh.advertise(bauStatusPub); + nh.advertise(alimStatPub); nh.subscribe(estimatedScoreSub); nh.subscribe(flagStatusSub); + nh.subscribe(armStateSub); nh.subscribe(bauSoftSub); lcd.clear(); @@ -200,12 +218,17 @@ void manage_supply_card_data(){ float curr5_value = (float) curr5_valueADC * CURR5_RATIO; float curr12_value = (float) curr12_valueADC * CURR12_RATIO; + alimStatMsg.voltage_12 = volt12_value; + alimStatMsg.voltage_5 = volt5_value; + alimStatMsg.current_12 = curr12_value; + alimStatMsg.current_5 = curr5_value; } void loop() { + // TODOOOOOOO : CHANGE if (!nh.connected()) { @@ -280,6 +303,8 @@ void loop() bauStatusMsg.data = bauStatus; bauStatusPub.publish(&bauStatusMsg); + manage_supply_card_data(); + alimStatPub.publish(&alimStatMsg); lastMessageTime = millis(); nh.spinOnce();