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