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; }