//callback that will run if the sensor value changes by more than the OnSensorChange trigger.
//Index - Index of the sensor that generated the event, Value - the sensor read value
int AnalogInputHandler(CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int Value)
{
//sensorValue 0-1000 ==> 0-5V
// Update power data
if(batteryPort == Index && powerdata_pub)
{
corobot_msgs::PowerMsg powerdata;
powerdata.volts = (float) (Value - 500) * 0.0734;
//The Min and Max present here are for the Nimh battery as it is the only one type of battery sold with a Corobot at the moment
powerdata.min_volt = 10.0;
powerdata.max_volt = 14.2;
powerdata_pub.publish(powerdata);
}
else if(irFrontPort == Index && irData_pub) // Update IR data
{
corobot_msgs::SensorMsg data;
data.type = data.INFRARED_FRONT;
data.index = Index;
data.value = irVoltageToDistance((float) Value / 200.0);
irData_pub.publish(data);
}
else if(irBackPort == Index && irData_pub) // Update IR data
{
corobot_msgs::SensorMsg data;
data.type = data.INFRARED_REAR;
data.index = Index;
data.value = irVoltageToDistance((float) Value / 200.0);
irData_pub.publish(data);
}
//sonar
else if(Index >= firstSonarInput && Index <= lastSonarInput && sonar_pub)
{
corobot_msgs::SensorMsg data;
data.type = data.ULTRASOUND;
data.index = Index;
data.value = sonarVoltageToMeters((float) Value / 200.0);
sonar_pub.publish(data);
}
else if(other_pub) // We don't know what sensor it is, but we publish
{
corobot_msgs::SensorMsg data;
data.type = data.OTHER;
data.value = Value;
data.index = Index;
other_pub.publish(data);
}
interfaceKitError = 0;
return 0;
}
/** @brief Function that will manage the sonars and acquire their data
* this function is called every around 50ms
*/
int sendSonarResult()
{
corobot_msgs::RangeSensor data;
data.type = data.ULTRASOUND;
data.numberSensors = lastSonarInput + 1 - firstSonarInput;
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
ros::Duration(0.002).sleep(); // sleep for 2ms
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
//Acquire the data and transform them into values in meters
for(int i = firstSonarInput; i<= lastSonarInput;i++)
{
data.range.push_back(sonarVoltageToMeters(analog_inputs_value[i]));
}
sonar_pub.publish(data);
return 0;
}
