int MotorController::regHanlders(void)
{
printf("MotorController::regHandlers\n");
#if (!DEBUG_MODE_PC)
int result;
const char *err;
#endif
//Declare a motor control handle
motorCtrlHandle = 0;
//create the motor control object
CPhidgetMotorControl_create(&motorCtrlHandle);
//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)motorCtrlHandle, MotorAttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motorCtrlHandle, MotorDetachHandler, NULL);
CPhidget_set_OnError_Handler( (CPhidgetHandle)motorCtrlHandle, MotorErrorHandler, NULL);
//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(motorCtrlHandle, MotorInputChangeHandler, 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(motorCtrlHandle, MotorVelocityChangeHandler, 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(motorCtrlHandle, MotorCurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motorCtrlHandle, -1);
//get the program to wait for a motor control device to be attached
printf("Waiting for MotorControl to be attached....\n");
#if (!DEBUG_MODE_PC)
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)motorCtrlHandle, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
#endif
//Display the properties of the attached motor control device
MotorDisplayProperties(motorCtrlHandle);
//all done, exit
return 0;
}
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;
}
int motorcontrol_simple()
{
int result;
const char *err;
//Declare a motor control handle
CPhidgetMotorControlHandle motoControl = 0;
//CPhidgetMotorControlHandle motoControl2 = 1;
//create the motor control object
CPhidgetMotorControl_create(&motoControl);
//CPhidgetMotorControl_create(&motoControl2);
//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);
/* CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl2, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl2, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl2, ErrorHandler, NULL);*/
//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);
//CPhidgetMotorControl_set_OnInputChange_Handler (motoControl2, 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);
//CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl2, 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);
//CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl2, CurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motoControl, -1);
//CPhidget_open((CPhidgetHandle)motoControl2, -1);
//get the program to wait for a motor control device to be attached
printf("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;
}
/*if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl2, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment 2: %s\n", err);
return 0;
}*/
//Display the properties of the attached motor control device
display_properties(motoControl);
//display_properties(motoControl2);
//read motor control event data
printf("Reading.....\n");
//keep displaying motor control event data until user input is read
printf("Press any key to continue\n");
getchar();
//Control the motor a bit.
//Step 1: increase acceleration to 50, set target sped at 100
/*CPhidgetMotorControl_setAcceleration (motoControl, 0, -50.00);
CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 100.00);
printf("Press any key to continue\n");
getchar();*/
//Step 2: Set acceleration to 100, decrease target speed to 75
CPhidgetMotorControl_setAcceleration (motoControl, 0, -100.00);
CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 100.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 70.00);
printf("Press any key to continue\n");
getchar();
//Step 3: Stop the motor by decreasing speed to 0;
CPhidgetMotorControl_setVelocity (motoControl, 0, 0.00);
CPhidgetMotorControl_setAcceleration (motoControl, 0, 0.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 0.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 0.00);
printf("Press any key to end\n");
getchar();
//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
printf("Closing...\n");
CPhidget_close((CPhidgetHandle)motoControl);
CPhidget_delete((CPhidgetHandle)motoControl);
/*CPhidget_close((CPhidgetHandle)motoControl2);
CPhidget_delete((CPhidgetHandle)motoControl2);*/
//all done, exit
return 0;
}
bool attach(
CPhidgetMotorControlHandle &phid,
int serial_number)
{
// create the object
CPhidgetMotorControl_create(&phid);
// 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)phid,
AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid,
DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)phid,
ErrorHandler, NULL);
// 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 (phid,
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 (phid,
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 (phid,
CurrentChangeHandler,
NULL);
//open the device for connections
CPhidget_open((CPhidgetHandle)phid, serial_number);
// get the program to wait for an motor control
// device to be attached
if (serial_number == -1) {
ROS_INFO("Waiting for Motor Control HC Phidget " \
"to be attached....");
}
else {
ROS_INFO("Waiting for Motor Control HC Phidget " \
"%d to be attached....", serial_number);
}
int result;
if((result =
CPhidget_waitForAttachment((CPhidgetHandle)phid,
10000)))
{
const char *err;
CPhidget_getErrorDescription(result, &err);
ROS_ERROR("Problem waiting for motor " \
"attachment: %s", err);
return false;
}
else return true;
}
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);
}