int ENCODER2_PositionChangeHandler(CPhidgetEncoderHandle ENC,
void *usrptr, int Index, int Time, int RelativePosition)
{
int Position;
CPhidgetEncoder_getPosition(ENC, Index, &Position);
// printf("Encoder #%i - Position: %5d -- Relative Change %2d -- Elapsed Time: %5d \n", Index, Position, RelativePosition, Time);
encoderPos[1]= -Position;
return 0;
}
int CCONV PositionChangeHandler(CPhidgetEncoderHandle ENC, void *usrptr, int Index, int Time, int RelativePosition)
{
int Position;
double globalTime ;
CPhidgetEncoder_getPosition(ENC, Index, &Position);
if (Index==0)
{
encTime0 +=((double)Time)/1000000.0;
globalTime = encTime0;
}
else
{
encTime1 +=((double)Time)/1000000.0;
globalTime = encTime1;
}
printf("Encoder #%i - Position: %5d -- Relative Change %2d -- Elapsed Time: %5d \n",
Index, Position, RelativePosition, Time);
fprintf(file,"%i \t %5d \t %2d \t %5d \t %f \n", Index, Position, RelativePosition, Time,globalTime);
return 0;
}
/**
* @brief Publish the data on their corresponding topics
*/
int publish_data(){
if(spatial_good)
{
corobot_msgs::spatial spatial;
sensor_msgs::Imu imu;
spatial.acc1 = acc[0];
spatial.acc2 = acc[1];
spatial.acc3 = acc[2];
spatial.ang1 = ang[0];
spatial.ang2 = ang[1];
spatial.ang3 = ang[2];
spatial.mag1 = mag[0];
spatial.mag2 = mag[1];
spatial.mag3 = mag[2];
imu.header.frame_id = "base_link";
imu.header.stamp = ros::Time::now();
imu.orientation = tf::createQuaternionMsgFromRollPitchYaw((atan2(dCMMatrix[2][1],dCMMatrix[2][2]) + rollOffset),(-asin(dCMMatrix[2][0]) + pitchOffset), (atan2(dCMMatrix[1][0],dCMMatrix[0][0])));
imu.angular_velocity.x = ang[0];
imu.angular_velocity.y = ang[1];
imu.angular_velocity.z = ang[2];
imu.linear_acceleration.x = acc[0];
imu.linear_acceleration.y = acc[1];
imu.linear_acceleration.z = acc[2];
spatial_pub.publish(spatial);
imu_pub.publish(imu);
spatialError = 0;
}
if(m_encodersGood) //the position is 4times the number of encoder counts.
{
corobot_msgs::PosMsg posdata;
// prepare to read the encoder status
int phidgetEncoderStatus = 0;
CPhidget_getDeviceStatus((CPhidgetHandle) m_leftEncoder,
&phidgetEncoderStatus);
if (phidgetEncoderStatus != 0)
{
int value;
CPhidgetEncoder_getPosition(m_leftEncoder, m_leftEncoderNumber, &value);
posdata.px = -value;
printf("Left encoder value = %d", value);
}
else
{
encoderError = 2;
}
phidgetEncoderStatus = 0;
CPhidget_getDeviceStatus((CPhidgetHandle) m_rightEncoder,
&phidgetEncoderStatus);
if (phidgetEncoderStatus != 0)
{
int value;
CPhidgetEncoder_getPosition(m_rightEncoder, m_rightEncoderNumber, &value);
// we negate this value since the right side motors turn the opposite direction
// to the left side
posdata.py = value;
}
else
{
encoderError = 2;
}
posdata.header.stamp = ros::Time::now();
posdata_pub.publish(posdata);
encoderError = 0;
}
else
{
encoderError = 1;
}
if (m_validAnalogs){
////////////////////////////
// Update power data
if(batteryPort != -1)
{
corobot_msgs::PowerMsg powerdata;
powerdata.volts = (float) (analog_inputs_value[batteryPort] - 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);
}
////////////////////////////
// Update IR data
if(irFrontPort != -1 || irBackPort != -1)
{
corobot_msgs::IrMsg irData;
if(irFrontPort != -1)
irData.voltage1=(float) analog_inputs_value[irFrontPort] / 200.0;
else
irData.voltage1=0;
if(irBackPort != -1)
irData.voltage2=(float) analog_inputs_value[irBackPort] / 200.0;
else
irData.voltage2=0;
irData.range1=irVoltageToDistance(irData.voltage1);
irData.range2=irVoltageToDistance(irData.voltage2);
irData_pub.publish(irData);
}
if(gripperPort != -1)
{
corobot_msgs::GripperMsg gripperData; // Gripper connect to the 4th analog input port?
gripperData.state=analog_inputs_value[gripperPort];
gripper_pub.publish(gripperData);
}
interfaceKitError = 0;
}
if (m_validDigitals)
{
////////////////////////////
// Update the bumper data
corobot_msgs::BumperMsg bumper_data;
if (m_rearBumperPresent)
{
bumper_data.bumpers_count=4;
bumper_data.value0=(digital_inputs_value) & (0x01<<0);
bumper_data.value1=(digital_inputs_value) & (0x01<<1);
bumper_data.value2=(digital_inputs_value) & (0x01<<2);
bumper_data.value3=(digital_inputs_value) & (0x01<<3);
}
else {
bumper_data.bumpers_count=2;
bumper_data.value0=(digital_inputs_value) & (0x01<<0);
bumper_data.value1=(digital_inputs_value) & (0x01<<1);
}
bumper_pub.publish(bumper_data);
interfaceKitError = 0;
}
return 0;
}