#include "ros/ros.h"
#include <stdio.h>
#include "corobot_srvs/SetOdom.h"
#include "corobot_msgs/PosMsg.h"
#include "corobot_msgs/PowerMsg.h"
#include "corobot_msgs/IrMsg.h"
#include "corobot_msgs/BumperMsg.h"
#include "corobot_msgs/GripperMsg.h"
#include "corobot_msgs/phidget_info.h"
#include "corobot_msgs/spatial.h"
#include "corobot_msgs/RangeSensor.h"
#include "sensor_msgs/Imu.h"
#include <phidget21.h>
#include <tf/transform_datatypes.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

Include dependency graph for corobot_phidget.cpp:

Defines
#define	GRAVITY 9.81
Functions
void	AnglesDriftCorrection (float AccelerationVector[3])
void	AnglesMatrixUpdate (float gyroscopeVector[3], int gyroscopeTimestamp)
void	AnglesNormalize ()
int	AttachHandler (CPhidgetHandle IFK, void *userptr)
double	constrain (double value, double min, double max)
int	DetachHandler (CPhidgetHandle IFK, void *userptr)
void	detectGravity (double gravity[3], float acceleration[3])
void	encoderAttach (const int which)
	function that handles registering the an encoder has been attached
int	ErrorHandler (CPhidgetHandle IFK, void userptr, int ErrorCode, const char unknown)
int	InputChangeHandler (CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int State)
	callback that will run if an input changes. Index - Index of the input that generated the event, State - boolean (0 or 1) representing the input state (on or off)
int	interfacekit_simple ()
static float	irVoltageToDistance (float volts)
	Description: Convert a voltage from the Phidgets 8/8/8 board into a distance, for the Infrared sensors return 25.4 meters as a distance if the value is out of range otherwise returns a valid distance in meters.
bool	isEqual (double vector1[3], double vector2[3])
int	LeftEncoderAttach (CPhidgetHandle phid, void *userPtr)
	callback invoked automatically when the left encoder is detected
int	main (int argc, char *argv[])
void	matrixMultiply (double MatrixOut[3][3], double MatrixIn1[3][3], double MatrixIn2[3][3])
int	OutputChangeHandler (CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int State)
void	phidget_encoder_diagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat)
void	phidget_ik_diagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat)
void	phidget_spatial_diagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat)
int	publish_data ()
	Publish the data on their corresponding topics.
int	RightEncoderAttach (CPhidgetHandle phid, void *userPtr)
	callback invoked automatically when the right encoder is detected
int	sendSonarResult ()
	Function that will manage the sonars and acquire their data this function is called every around 50ms.
int	SensorChangeHandler (CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int Value)
bool	SetOdom (corobot_srvs::SetOdom::Request &req, corobot_srvs::SetOdom::Response &res)
	set a value to the odometers
static double	sonarVoltageToMeters (int value)
int	SpatialDataHandler (CPhidgetSpatialHandle spatial, void userptr, CPhidgetSpatial_SpatialEventDataHandle data, int count)
double	ToRad (double value)
void	vectorAddition (double vectorOut[3], double vectorIn1[3], double vectorIn2[3])
void	vectorCrossProduct (double vectorOut[3], double vectorIn1[3], double vectorIn2[3])
double	vectorDotProduct (double vector1[3], double vector2[3])
void	vectorScale (double vectorOut[3], double vectorIn[3], double scale2)
Variables
float	acc [3] = {0,0,0}
int	analog_inputs_value [8] = {0,0,0,0,0,0,0,0}
float	ang [3] = {0,0,0}
int	batteryPort = 0
ros::Publisher	bumper_pub
int	bwOutput = -1
double	dCMMatrix [3][3]
char	digital_inputs_value = 0
int	encoderError = 0
bool	encoders_inverted
int	firstSonarInput = -1
double	gravity [3]
double	gravityEpsilon = 0.07
ros::Publisher	gripper_pub
int	gripperPort = 3
double	gyroscopeVectorCorrected [3]
CPhidgetInterfaceKitHandle	ifKit = 0
ros::Publisher	imu_pub
double	integratorVector [3]
int	interfaceKitError = 0
int	irBackPort = 2
ros::Publisher	irData_pub
int	irFrontPort = 1
double	kiRollpitch = 0.000010
double	kpRollpitch = 0.015
double	lastGravityVectorDetected [3] = {0,0,0}
int	lastSonarInput = -1
bool	m_encoder1Seen = false
bool	m_encoder2Seen = false
bool	m_encodersGood = false
CPhidgetEncoderHandle	m_leftEncoder
int	m_leftEncoderNumber
bool	m_rearBumperPresent = false
CPhidgetEncoderHandle	m_rightEncoder
int	m_rightEncoderNumber
bool	m_validAnalogs = false
bool	m_validDigitals = false
float	mag [3] = {0,0,0}
bool	motors_inverted
bool	phidget888_connected
bool	phidget_encoder_connected
double	pitchOffset
ros::Publisher	posdata_pub
ros::Publisher	powerdata_pub
double	proportionalVector [3]
double	rollOffset
ros::Publisher	sonar_pub
bool	sonarsPresent = false
CPhidgetSpatialHandle	spatial = 0
bool	spatial_good = false
ros::Publisher	spatial_pub
int	spatialError = 0
int	strobeOutput = -1
int	timestampPreviousCall
int	validAccelerationVectorsNecessaryToDetectGravity = 10
int	validGravityCounter = 0

Define Documentation

#define GRAVITY 9.81

Definition at line 24 of file corobot_phidget.cpp.

Function Documentation

void AnglesDriftCorrection ( float AccelerationVector[3] )

Definition at line 676 of file corobot_phidget.cpp.

void AnglesMatrixUpdate	(	float	gyroscopeVector[3],
		int	gyroscopeTimestamp
	)

Definition at line 549 of file corobot_phidget.cpp.

void AnglesNormalize ( )

Definition at line 606 of file corobot_phidget.cpp.

int AttachHandler	(	CPhidgetHandle	IFK,
		void *	userptr
	)

Definition at line 99 of file corobot_phidget.cpp.

double constrain	(	double	value,
		double	min,
		double	max
	)

Definition at line 530 of file corobot_phidget.cpp.

int DetachHandler	(	CPhidgetHandle	IFK,
		void *	userptr
	)

Definition at line 112 of file corobot_phidget.cpp.

void detectGravity	(	double	gravity[3],
		float	acceleration[3]
	)

Definition at line 726 of file corobot_phidget.cpp.

void encoderAttach ( const int which )

function that handles registering the an encoder has been attached

Parameters:

which indicates which encoder was seen (1 or 2)

Definition at line 132 of file corobot_phidget.cpp.

int ErrorHandler	(	CPhidgetHandle	IFK,
		void *	userptr,
		int	ErrorCode,
		const char *	unknown
	)

Definition at line 125 of file corobot_phidget.cpp.

int InputChangeHandler	(	CPhidgetInterfaceKitHandle	IFK,
		void *	usrptr,
		int	Index,
		int	State
	)

callback that will run if an input changes. Index - Index of the input that generated the event, State - boolean (0 or 1) representing the input state (on or off)

Definition at line 434 of file corobot_phidget.cpp.

int interfacekit_simple ( )

Definition at line 830 of file corobot_phidget.cpp.

static float irVoltageToDistance ( float volts ) [static]

Description: Convert a voltage from the Phidgets 8/8/8 board into a distance, for the Infrared sensors return 25.4 meters as a distance if the value is out of range otherwise returns a valid distance in meters.

Definition at line 227 of file corobot_phidget.cpp.