void ICACHE_FLASH_ATTR setPirFanActive(enum pir_t pir) {
if (PIR1 <= pir && pir <= PIR2) {
pirFanStatus[pir] = true;
if (pirFanStatus[pir] != oldPirFanstatus[pir]) {
TESTP("PIR Fan:%d ", pir);
publishSensorData(SENSOR_FAN_PIR_ACTIVE1 - 1 + pir, "PIR FAN ON", "1");
oldPirFanstatus[pir] = true;
}
os_timer_disarm(&pirFanTmer[pir]);
os_timer_setfn(&pirFanTmer[pir], (os_timer_func_t *) pirFanTimeoutCb, pir);
os_timer_arm(&pirFanTmer[pir], sysCfg.settings[SETTING_FAN_PIR1_ON_TIME-1+pir]*1000*60, false); // Minutes
} else {
ERRORP("Bad PIR # %d", pir);
}
}
static void setPirLightActive(enum pir_t pir) {
if (PIR1 <= pir && pir <= PIR2) {
pirLightStatus[pir] = true;
if (pirLightStatus[pir] != oldPirLightstatus[pir]) {
TESTP("PIR Lt:%d ", pir);
publishSensorData(SENSOR_LIGHT_PIR_ACTIVE1 - 1 + pir, "PIR LIGHT ON", "1");
oldPirLightstatus[pir] = true;
}
os_timer_disarm(&pirLightTmer[pir]);
os_timer_setfn(&pirLightTmer[pir], (os_timer_func_t *) pirLightTimeoutCb, pir);
os_timer_arm(&pirLightTmer[pir], sysCfg.settings[SETTING_LIGHT_PIR1_ON_TIME-1+pir]*1000*60, false); // Minutes
} else {
ERRORP("Bad PIR # %d", pir);
}
}
static void ICACHE_FLASH_ATTR pirFanTimeoutCb(uint32 args) {
enum pir_t pir = args;
TESTP("pirFanTimeoutCb %d\n", pir);
publishSensorData(SENSOR_FAN_PIR_ACTIVE1 - 1 + pir, "PIR FAN ON", "0");
oldPirFanstatus[pir] = pirFanStatus[pir] = false;
}
static void ICACHE_FLASH_ATTR pirLightTimeoutCb(uint32 args) {
enum pir_t pir = args;
TESTP("pirLightTimeoutCb %d\n", pir);
publishSensorData(SENSOR_LIGHT_PIR_ACTIVE1 - 1 + pir, "PIR LIGHT ON", "0");
oldPirLightstatus[pir] = pirLightStatus[pir] = false;
}
//main ros code
int main(int argc, char **argv)
{
//setup topic names
std::string twist_topic;
//setup node and image transport
ros::init(argc, argv, "hardware_interface");
ros::NodeHandle node;
//setup dynamic reconfigure gui
dynamic_reconfigure::Server<hardware_interface::hardware_interface_paramsConfig> srv;
dynamic_reconfigure::Server<hardware_interface::hardware_interface_paramsConfig>::CallbackType f;
f = boost::bind(&setparamsCallback, _1, _2);
srv.setCallback(f);
//Initialize serial port
motor_port.serial_open(params.serial_port.c_str(), 19200);
//create brodcaster
tf::TransformBroadcaster odom_broadcaster;
//initalize odometry
g_odom_x = 0.0;
g_odom_y = 0.0;
g_odom_th = 0.0;
g_vx = 0.0;
g_vy = 0.0;
g_v_theta = 0.0;
//initalize variables for max accel
for(int i = 0; i < 3; i++)
{
last_wheel_speed_[i] = 0;
}
vel_last_time_ = ros::Time::now();
//get twist topic name
twist_topic = node.resolveName("twist");
//check to see if user has defined an image to subscribe to
if (twist_topic == "/twist")
{
ROS_WARN("hardware_interface: twist has not been remapped! Typical command-line usage:\n"
"\t$ ./hardware_interface twist:=<image topic> [transport]");
}
//create twist subscription
twist_sub = node.subscribe( twist_topic , 1, motionCallback );
//advertise publishers
sensor_pub = node.advertise<hardware_interface::BaseData>("hardware_interface/sensor_data", 5);
odom_pub = node.advertise<nav_msgs::Odometry>("/odom", 1000);
//collect motor base sensor data at this rate
ros::Rate loop_rate(SENSOR_RATE);
while (ros::ok())
{
//check callbacks
ros::spinOnce();
//handel timeout
if( watchdog_timer_ < ros::Time::now() && motors_enabled_)
{
//have not receved a new message so stop robot
ROS_WARN("Watchdog timed out!");
//killMotors();
motors_enabled_ = false;
}
//collect and publish sensor data
publishSensorData();
//publish odometry with no movement
updateOdomMsg(g_vx, g_vy, g_v_theta);
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//publish odom message
odom_pub.publish(odom_msg);
//wait for next spin
loop_rate.sleep();
}
return 0;
}
