CPhidgetAdvancedServoHandle InitializeServos(int n, int* channels, int grip_channel, double* AccelThrottle, double* VelLimThrottle, double grip_AccelThrottle, double grip_VelLimThrottle) {
int result;
const char *err;
double minAccel, maxAccel, minVel, maxVel;
/* If servo is already defined, return. */
if (servo != NULL) {
return servo;
}
/* Create the advanced servo object */
CPhidgetAdvancedServo_create(&servo);
/* Set the handlers to be run when the device is plugged in (opened) from software, unplugged (closed) from software, or generates an error. */
CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
/* Open the device for connections */
CPhidget_open((CPhidgetHandle)servo, -1);
LOG(logINFO) << "Waiting for servos to be attached...";
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 5000))) {
CPhidget_getErrorDescription(result, &err);
LOG(logERROR) << "Problem waiting for attachment: "<< err;
return 0;
}
/* Set the acceleration and velocity limits of each joint angle actuator */
for (int i = 0; i < n; i++) {
CPhidgetAdvancedServo_getAccelerationMin(servo, channels[i], &minAccel);
CPhidgetAdvancedServo_getAccelerationMax(servo, channels[i], &maxAccel);
CPhidgetAdvancedServo_setAcceleration(servo, channels[i], AccelThrottle[i]*(maxAccel - minAccel) + minAccel );
CPhidgetAdvancedServo_getVelocityMin(servo, channels[i], &minVel);
CPhidgetAdvancedServo_getVelocityMax(servo, channels[i], &maxVel);
CPhidgetAdvancedServo_setVelocityLimit(servo, channels[i], VelLimThrottle[i]*(maxVel - minVel) + minVel );
}
/* Set the acceleration and velocity limits of the end effector */
CPhidgetAdvancedServo_getAccelerationMin(servo, grip_channel, &minAccel);
CPhidgetAdvancedServo_getAccelerationMax(servo, grip_channel, &maxAccel);
CPhidgetAdvancedServo_setAcceleration(servo, grip_channel, grip_AccelThrottle*(maxAccel - minAccel) + minAccel );
CPhidgetAdvancedServo_getVelocityMin(servo, grip_channel, &minVel);
CPhidgetAdvancedServo_getVelocityMax(servo, grip_channel, &maxVel);
CPhidgetAdvancedServo_setVelocityLimit(servo, grip_channel, grip_VelLimThrottle*(maxVel - minVel) + minVel );
return (CPhidgetAdvancedServoHandle)servo;
}
int setup()
{
init_debugging();
int result, numSensors, i;
const char *err;
//handles *Handles;
//Declare an InterfaceKit handle
// Setup the IFKit
CPhidgetInterfaceKit_create(&ifKit);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, IKInputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, IKSensorChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, IKOutputChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)ifKit, -1);
//get the program to wait for an interface kit device to be attached
SetupLog("Waiting for interface kit to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached interface kit device
IKDisplayProperties(ifKit);
// Setup motoControl
CPhidgetMotorControl_create(&motoControl);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);
CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, MCInputChangeHandler, NULL);
CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, MCVelocityChangeHandler, NULL);
CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, MCCurrentChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)motoControl, -1);
SetupLog("Waiting for MotorControl to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
MCDisplayProperties(motoControl);
CPhidgetMotorControl_setAcceleration (motoControl, 0, 50.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00);
// Setup AdvancedServo
CPhidgetAdvancedServo_create(&servo);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
CPhidgetAdvancedServo_set_OnPositionChange_Handler(servo, ASPositionChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)servo, -1);
SetupLog("Waiting for Phidget to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached device
ASDisplayProperties(servo);
CPhidgetAdvancedServo_setEngaged(servo, 0, 1);
state.ServoPosition = 0;
sensor.RightWhisker = 0;
sensor.LeftWhisker = 0;
sensor.FrontFacingIR = 0;
sensor.TopIR = 0;
state.AverageBaseLight = (float)10000;
sensor.TopLeftLight = 0;
sensor.TopRightLight = 0;
state.flashWasOn = 0;
state.wasOnBlackInLastIteration = 0;
sensor.SpinSensor = 10.0;
state.expectedMovement = None;
state.expectedFor = 0;
state.exitTrialCounter = 0;
state.stuckCounter = 0;
state.previousState = 2;
gettimeofday(&state.lastFlashSighted, NULL);
//#ifdef FREQUENCY
//state.frequency = FREQUENCY;
//#endif
#ifndef NO_POWERLIB
power_button_reset();
while(power_button_get_value()==0)
{
sleep(1);
}
#endif
return 0;
}
Controller::Controller()
: motoControl(0)
, speed(40)
, speedLeftFactor(1.5)
, speedRightFactor(1.5)
, accelLeftFactor(1.5)
, accelRightFactor(1.5)
, rotationOnSpotSpeed(100)
, accel(15)
, backwardTurnFastFactor(1.5)
, backwardTurnSlowFactor(-1.5)
, servo(0)
, servoOpen(35.00)
, servoClosed(150.00)
/*, speed(100)
, speedLeftFactor(1.0)
, speedRightFactor(1.0)
, accelLeftFactor(1.0)
, accelRightFactor(1.0)
, rotationOnSpotSpeed(40)
, accel(100)
, backwardTurnFastFactor(1)
, backwardTurnSlowFactor(-1)*/
{
//create the motor control object
CPhidgetMotorControl_create(&motoControl);
//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);
ifKit = 0;
//create the InterfaceKit object
CPhidgetInterfaceKit_create(&ifKit);
//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);
//open the interfacekit for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
CPhidgetAdvancedServo_create(&servo);
CPhidget_open((CPhidgetHandle)servo, -1);
int result;
const char *err;
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
{
CPhidget_getErrorDescription(result, &err);
}
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, InputChangeHandler, NULL);
//Registers a callback that will run if a motor changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, VelocityChangeHandler, NULL);
//Registers a callback that will run if the current draw changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, CurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motoControl, -1);
//get the program to wait for a motor control device to be attached
CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000);
CPhidgetAdvancedServo_setEngaged(servo, 0, 1);
CPhidgetAdvancedServo_setPosition (servo, 0, 40.00);
}
int main(int argc, char* argv[])
{
int result;
const char *err;
double position;
// Init ROS node overriding SIGINT (roslaunch, ctrl-c)
// and XMLRPC shutdown (rosnode kill)
// See also ticket
// https://code.ros.org/trac/ros/ticket/3417
// as a unified solution might appear in future ROS versions
ros::init( argc, argv, "servo_mast", ros::init_options::NoSigintHandler );
signal( SIGINT, mySigIntHandler );
// Override shutdown
ros::XMLRPCManager::instance()->unbind( "shutdown" );
ros::XMLRPCManager::instance()->bind( "shutdown", shutdownCallback );
ros::NodeHandle nodeHandle;
//create the advanced servo object
CPhidgetAdvancedServo_create( &servo );
// Set the handlers to be run when the device is plugged in or opened from software,
// unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler( (CPhidgetHandle)servo, AttachHandler, NULL );
CPhidget_set_OnDetach_Handler( (CPhidgetHandle)servo, DetachHandler, NULL );
CPhidget_set_OnError_Handler( (CPhidgetHandle)servo, ErrorHandler, NULL );
// Registers a callback that will run when the motor position is changed.
// Args:
// the handle for the Phidget
// the function that will be called
// arbitrary pointer that will be supplied to the callback function (may be NULL)
CPhidgetAdvancedServo_set_OnPositionChange_Handler( servo, PositionChangeHandler, NULL );
//open the device for connections
CPhidget_open( (CPhidgetHandle)servo, -1 );
//get the program to wait for an advanced servo device to be attached
printf("Waiting for Phidget to be attached....");
result = CPhidget_waitForAttachment( (CPhidgetHandle)servo, 10000 );
if( result ) {
CPhidget_getErrorDescription( result, &err );
printf( "Problem attaching Phidget: %s\n", err );
return 0;
}
else {
}
CPhidgetAdvancedServo_getAccelerationMin(servo, 0, &minAcc);
CPhidgetAdvancedServo_getAccelerationMax(servo, 0, &maxAcc);
CPhidgetAdvancedServo_getVelocityMin(servo, 0, &minVel);
CPhidgetAdvancedServo_getVelocityMax(servo, 0, &maxVel);
maxVel = maxVel/320;
maxAcc = maxAcc/12800;
CPhidgetAdvancedServo_setAcceleration( servo, 0, maxAcc);
CPhidgetAdvancedServo_setVelocityLimit( servo, 0, maxVel);
display_properties( servo );
// Defaults. If user only publishes float64 messages, these will be used
//CPhidgetAdvancedServo_setAcceleration( servo, 0, minAcc*2/4 );
//CPhidgetAdvancedServo_setVelocityLimit( servo, 0, maxVel/8 );
publisher = nodeHandle.advertise<std_msgs::Float64>( "mast_float2", 100 );
publisher_2 = nodeHandle.advertise<servo_mast::mast_position>("mast_position", 100);
ros::Subscriber subscriber1 = nodeHandle.subscribe( "mast_turn", 1000, turnCallback );
//ros::Subscriber subscriber2 = nodeHandle.subscribe( "mast_float", 1000, floatCallback );
ros::Subscriber subscriber2 = nodeHandle.subscribe( "mast_float", 100, floatCallback );
while( !g_request_shutdown ) {
std_msgs::Float64 msg;
msg.data=serv_pos;
//publisher.publish(msg);
ros::spinOnce();
//usleep(10000);
//CPhidgetAdvancedServo_setEngaged( servo, 0, 1 );
//CPhidgetAdvancedServo_setPosition( servo, 0, 180 );
//printf("Motor: 0 > Current Position: %f\n", current_pos);
//usleep(100000);
usleep(3000);
}
printf( "Closing...\n" );
shutdownStuff();
return 0;
}
