Diff: variables/instanceHeader.hpp

Revision:

3:a4677501ae87

Parent:

2:0de4854743f7

--- a/variables/instanceHeader.hpp	Thu Jun 10 01:23:00 2021 +0000
+++ b/variables/instanceHeader.hpp	Tue Nov 30 08:13:05 2021 +0000
@@ -1,45 +1,74 @@
 #ifndef ZETA_STM_KINETIC_INSTANCEHEADER_H_
 #define ZETA_STM_KINETIC_INSTANCEHEADER_H_
 #include "mbed.h"
-#include "rosHeader.hpp"
-#include "moduleHeader.hpp"
-#include "callbackHeader.hpp"
+#include "src/rosHeader.hpp"
+#include "src/moduleHeader.hpp"
+#include "src/callbackHeader.hpp"
 #include "defineHeader.h"
-
+#include "configurations/robotConfig.h"
 /* COM variables ------------------------------------------------------------ */
-    #if (NO_ROS)
+#if (NO_ROS)
 extern mbed::Serial pc;
-    #else
+#else
 extern ros::NodeHandle nh;
-    #endif
+#endif
 extern mbed::Serial bt;
 
 /* ROS variables ------------------------------------------------------------ */
 // Publisher
+//zetabot_main::SonarArray US_msg;
+//ros::Publisher US_publisher("sonar", &US_msg);
+std_msgs::String version_msg;
+ros::Publisher version_publisher("stm_version", &version_msg);
+
+std_msgs::Float32MultiArray US_msg;
+ros::Publisher US_publisher("sonar", &US_msg);
+
+std_msgs::String Bluetooth_msg;
+ros::Publisher Bluetooth_publisher("autocharge_state_INO", &Bluetooth_msg);
+
+#if (ROBOT_TYPE == MODEL_D)
+//std_msgs::UInt8 water_level_msg;
+//ros::Publisher water_level_publisher("water_level", &water_level_msg);
+std_msgs::Bool water_level_msg;
+ros::Publisher water_level_publisher("water_level", &water_level_msg);
+#endif
+
+#if (ROBOT_TYPE == MODEL_I)
+std_msgs::Bool lidar_dusty_msg;
+ros::Publisher LidarDusty_publisher("LidarDusty", &lidar_dusty_msg);
+
+std_msgs::UInt8 emergency_stop_msg;
+ros::Publisher EmergencyStop_publisher("EmergencyStop", &emergency_stop_msg);
+#endif
+
 sensor_msgs::Imu imu_msg;
 ros::Publisher IMU_publisher("imu", &imu_msg);
 
 std_msgs::Bool EStop_msg;
 ros::Publisher EStop_publisher("estop", &EStop_msg);
+
 std_msgs::Bool Bumper_msg;
 ros::Publisher Bumper_publisher("bumper", &Bumper_msg);
 
-zetabot_main::SonarArray US_msg;
-ros::Publisher US_publisher("sonar", &US_msg);
-
-std_msgs::String Bluetooth_msg;
-ros::Publisher Bluetooth_publisher("autocharge_state_INO", &Bluetooth_msg);
 
 // Subscriber
-
-//void UVCcontrolCB(const std_msgs::Bool& msg);
-//ros::Subscriber<std_msgs::Bool> UVCcontrol_subscriber("uvc_control_command",UVCcontrolCB);
-//std_msgs::Bool UVCcontrolMsg;
-
 void BluetoothCB(const std_msgs::UInt8& msg);
 ros::Subscriber<std_msgs::UInt8> Bluetooth_subscriber("autocharge_state_NUC", &BluetoothCB);
 
+void SsrTestCB(const std_msgs::Bool& msg);
+ros::Subscriber<std_msgs::Bool> SsrTest_subscriber("SsrControlTest", &SsrTestCB);
 
+#if (ROBOT_TYPE == MODEL_I)
+void WarningFieldSelectCB(const std_msgs::UInt8& msg);
+ros::Subscriber<std_msgs::UInt8> WarningFieldSelect_subscriber("WarningFieldSelect", &WarningFieldSelectCB);
+
+void IgnoreWarningFieldCB(const std_msgs::UInt8&);
+ros::Subscriber<std_msgs::UInt8> IgnoreWarningField_subscriber("IgnoreWarningField", &IgnoreWarningFieldCB);
+
+void ScrubberControlCB(const std_msgs::Bool& msg);
+ros::Subscriber<std_msgs::Bool> ScrubberControl_subscriber("ScrubberControl", &ScrubberControlCB);
+#endif
 /* Threads and Timers ------------------------------------------------------- */
 Thread gThread[NUM_THREAD];
 
@@ -47,23 +76,37 @@
 
 
 /* Modules ------------------------------------------------------------------ */
-// Driving non-related modules
-//extern UVC uvc;
-RELAY relay(CHARGE_RELAYP,CHARGE_RELAYN);
-// Driving related modules
-
 MPU9250_SPI mpu(IMU_MOSI, IMU_MISO, IMU_SCK, IMU_NCS, IMU_INT);
 
-HCSR04 sonar[NUM_SONAR] = {
-    HCSR04(SONAR_ECHO0, SONAR_TRIG0, SONAR_FILTER_WS,1), HCSR04(SONAR_ECHO1, SONAR_TRIG0, SONAR_FILTER_WS,2),
-    HCSR04(SONAR_ECHO2, SONAR_TRIG0, SONAR_FILTER_WS,3), HCSR04(SONAR_ECHO3, SONAR_TRIG0, SONAR_FILTER_WS,4),
-    HCSR04(SONAR_ECHO4, SONAR_TRIG0, SONAR_FILTER_WS,5), HCSR04(SONAR_ECHO5, SONAR_TRIG0, SONAR_FILTER_WS,6),
-    HCSR04(SONAR_ECHO6, SONAR_TRIG0, SONAR_FILTER_WS,7), HCSR04(SONAR_ECHO7, SONAR_TRIG0, SONAR_FILTER_WS,8),
-    HCSR04(SONAR_ECHO8, SONAR_TRIG0, SONAR_FILTER_WS,9), HCSR04(SONAR_ECHO9, SONAR_TRIG0, SONAR_FILTER_WS,10)
-    };
-SONAR_MANAGER sonar_manager(SONAR_TRIG0, sonar, NUM_SONAR);
+#if ((ROBOT_TYPE == MODEL_C) || (ROBOT_TYPE == MODEL_D))
+ChargingControl charging_control(CHARGE_RELAYP, CHARGE_RELAYN);
+DigitalIn estop(EMERGENCY_STOP);
+PinName echo[NUM_SONAR] = {RS_ECH01, RS_ECH02, RS_ECH03, RS_ECH04, RS_ECH05, RS_ECH06, RS_ECH07, RU_ECH01, RU_ECH02, RU_ECH03};
+SONAR_MANAGER<NUM_SONAR> sonar_manager(echo, TRIG);
+#elif (ROBOT_TYPE == MODEL_I)
+PinName echo[NUM_SONAR] = {SONAR_LEFT, SONAR_RIGHT};
+SONAR_MANAGER<NUM_SONAR> sonar_manager(echo, SONAR_TRIG);
+#endif
 
-EStop estop(EMERGENCY);
+#if (ROBOT_TYPE == MODEL_D)
+//DigitalIn level_blue(LEVEL_SENSE1);
+//DigitalIn level_yellow(LEVEL_SENSE2);
+DigitalIn level_sensor(LEVEL_SENSE);
+#endif
+
+#if (ROBOT_TYPE == MODEL_I)
+DigitalOut ChargingSsr(SSR_CTRL, 0);
+DigitalOut ScrubberControl(SCRUBBER_CTRL, 0);
+DigitalOut WaringFieldSelectPin1(LIDAR_WARNING1, 1);
+DigitalOut WaringFieldSelectPin2(LIDAR_WARNING2, 1);
+DigitalOut IgnoreWarningFieldPin1(IGNORE_WARNING1, 0);
+DigitalOut IgnoreWarningFieldPin2(IGNORE_WARNING2, 0);
+DigitalIn LidarDustSensingPin(LIDAR_DUSTSENSING);
+DigitalIn EmergencyStopPin(EMERGENCY_STOP);
+DigitalIn LidarStopPin(LIDAR_DETECT_OBSTACLE);
+#endif
+
+//EStop estop(EMERGENCY);
 
 /* For test ----------------------------------------------------------------- */