double DRobotServoMotor::getMotorPosition() {
double position;
CPhidgetServo_getPosition(servo, this->getIndex(), &position);
return position;
}
int servo_simple()
{
int result;
double curr_pos;
const char *err;
//Declare an servo handle
CPhidgetServoHandle servo = 0;
//create the servo object
CPhidgetServo_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.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetServo_set_OnPositionChange_Handler(servo, PositionChangeHandler, NULL);
//open the servo for device connections
CPhidget_open((CPhidgetHandle)servo, -1);
//get the program to wait for an servo device to be attached
printf("Waiting for Servo controller 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 servo device
display_properties(servo);
//read servo event data
printf("Reading.....\n");
//This example assumes servo motor is attached to index 0
//display current motor position
CPhidgetServo_getPosition(servo, 0, &curr_pos);
printf("Motor: 0 > Current Position: %f\n", curr_pos);
//keep displaying servo event data until user input is read
printf("Press any key to continue\n");
getchar();
//change the motor position
//valid range is -22 to 232
//we'll set it to a few random positions to move it around
//Step 1: Position 10.00
printf("Move to position 10.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 10.00);
//Step 2: Position 50.00
printf("Move to position 50.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 50.00);
//Step 3: Position 100.00
printf("Move to position 100.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 100.00);
//Step 4: Position 150.00
printf("Move to position 150.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 150.00);
//Step 5: Position 200.00
printf("Move to position 200.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 200.00);
//Step 6: Position 20.00
printf("Move to position 20.00. Press any key to Continue\n");
getchar();
CPhidgetServo_setPosition (servo, 0, 20.00);
//Step 7: Diseangage
printf("Disengage. Press any key to Continue\n");
getchar();
CPhidgetServo_setEngaged (servo, 0, 0);
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)servo);
CPhidget_delete((CPhidgetHandle)servo);
//all done, exit
return 0;
}
