int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_motor_gpio"); ros::NodeHandle n; ros::NodeHandle nh("~"); supplyVoltagePub = n.advertise<std_msgs::Float32>("/cl4_motor_gpio/supply_voltage", 1, true); motorCurrentPub = n.advertise<std_msgs::Float32MultiArray>("/cl4_motor_gpio/motor_current", 1, true); motorBackEMFPub = n.advertise<std_msgs::Float32MultiArray>("/cl4_motor_gpio/motor_back_emf", 1, true); //batteryStatePub = n.advertise<sensor_msgs::BatteryState>("/battery", 1, true); // Load parameters nh.getParam("serial", serialNumber); nh.param("enableBackEMF", enableBackEMF, false); if (attach(mcphid, serialNumber)) { display_properties(mcphid); initalized = true; ros::Timer timer = nh.createTimer(ros::Duration(0.5), timerCallback); ros::spin(); disconnect(mcphid); } return 0; }
// connect to a device. Use -1 for first available device void PhidgetConnector::connect(int serial_in, int _timeOut) { cout << "attempting connection to board #" << serial_in << "... " ; CPhidgetInterfaceKitHandle * thisIFKIT = new CPhidgetInterfaceKitHandle(); ifkits.push_back(thisIFKIT); CPhidgetInterfaceKit_create(thisIFKIT); if(bUseEvents) { cout << "using events..." << endl; CPhidget_set_OnError_Handler((CPhidgetHandle)*thisIFKIT, ErrorHandler, this); CPhidgetInterfaceKit_set_OnSensorChange_Handler(*thisIFKIT, SensorChangeHandler, this); } else { cout << "not using events..." << endl; } CPhidget_open((CPhidgetHandle)*thisIFKIT, serial_in); printf("Waiting for interface kit to be attached...."); int result; const char *err; int timeOut = 30000; if(_timeOut>0) timeOut = _timeOut * 1000; if((result = CPhidget_waitForAttachment((CPhidgetHandle)*thisIFKIT, timeOut))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment %s\n", err); } else { printf("sensor opened okay."); display_properties(*thisIFKIT); } }
int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_spatial"); ros::NodeHandle n; ros::NodeHandle nh("~"); int serial_number = -1; int data_rate = 16; nh.getParam("serial", serial_number); nh.getParam("rate", data_rate); std::string name = "spatial"; nh.getParam("name", name); if (serial_number==-1) { nh.getParam("serial_number", serial_number); } std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); int frequency = 30; nh.getParam("frequency", frequency); int ax_id = 0; nh.getParam("x_axis_id", ax_id); axis_id[0] = ax_id; ax_id = 1; nh.getParam("y_axis_id", ax_id); axis_id[1] = ax_id; ax_id = 2; nh.getParam("z_axis_id", ax_id); axis_id[2] = ax_id; if (attach(phid, serial_number, data_rate)) { display_properties(phid); const int buffer_length = 100; std::string topic_name = topic_path + name; if (serial_number > -1) { char ser[10]; sprintf(ser,"%d", serial_number); topic_name += "/"; topic_name += ser; } spatial_pub = n.advertise<phidgets::spatial_params>(topic_name, buffer_length); ros::Rate loop_rate(frequency); initialised = true; while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } disconnect(phid); } return 0; }
/* -------------------------------------------------------------------------- * * --------------- Compute the formal concepts by brute force --------------- * * -------------------------------------------------------------------------- */ int main(int argc, char const *argv[]) { for(int i = argc ; i-- ; ) printf( "%s\n", argv[ i ] ); char buffer[2][16]; FILE *fout; fout = fopen( "concepts.txt", "w+" ); // Build concepts. It does not matter whether we traverse the space of // objects or the space of properties. for( int i = 0 ; i < ( 1 << G ) ; ++i ) { unsigned p = properties( i ); unsigned o = objects( p ); // The concept is a pair (g, m) such that g. = m and .m = g. Equivalently // a formal concept is a pair (g, g.) such that .(g.) = g, i.e g is closed // in the concept matrix. I wonder if one can define a topology on this // discrete poset. if( o != i ) continue; display_objects( i, buffer[ 0 ] ); display_properties( p, buffer[ 1 ] ); // display_objects( ( ~i ) & ( ( 1 << G ) - 1 ), buffer[ 0 ] ); fprintf( fout, "%.*s\t%.*s\n", G, buffer[ 0 ], M, buffer[ 1 ] ); } fclose( fout ); return 0; }
int encoder_simple() { int result; const char *err; file=fopen("/home/dune/dataSaved/essai1/encoder","w"); //Declare an encoder handle CPhidgetEncoderHandle encoder = 0; //create the encoder object CPhidgetEncoder_create(&encoder); //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)encoder, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)encoder, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)encoder, 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 an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetEncoder_set_OnInputChange_Handler(encoder, InputChangeHandler, NULL); //Registers a callback that will run if the encoder changes. //Requires the handle for the Encoder, the function that will be called, //and an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetEncoder_set_OnPositionChange_Handler (encoder, PositionChangeHandler, NULL); CPhidget_open((CPhidgetHandle)encoder, -1); struct timeval now ; //get the program to wait for an encoder device to be attached printf("Waiting for encoder to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)encoder, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } gettimeofday(&now,NULL); encTime0 = now.tv_sec + ((double)now.tv_usec)/1000000.0; encTime1 = encTime0 ; //Display the properties of the attached encoder device display_properties(encoder); //read encoder event data printf("Reading.....\n"); //keep displaying encoder data until user input is read 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)encoder); CPhidget_delete((CPhidgetHandle)encoder); fclose(file); //all done, exit return 0; }
int tempsensor_simple() { int result; const char *err; //Declare an temperature sensor handle CPhidgetTemperatureSensorHandle temp = 0; //create the temperature sensor object CPhidgetTemperatureSensor_create(&temp); //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)temp, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)temp, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)temp, ErrorHandler, NULL); //Registers a callback that will run if the Temperature changes by more than the Temperature trigger. //Requires the handle for the Temperature Sensor, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetTemperatureSensor_set_OnTemperatureChange_Handler(temp, TemperatureChangeHandler, NULL); //open the temperature sensor for device connections CPhidget_open((CPhidgetHandle)temp, -1); //get the program to wait for an temperature sensor device to be attached printf("Waiting for TemperatureSensor to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)temp, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached accelerometer device display_properties(temp); //read temperature sensor event data printf("Reading.....\n"); //keep displaying temperature sensor event data until user input is read //modify the sensor sensitivity, index 1 is the thermocouple sensor, index 0 is the onboard or ambient sensor printf("Setting sensitivity of the thermocouple to 2.00. Press any key to continue\n"); getchar(); CPhidgetTemperatureSensor_setTemperatureChangeTrigger (temp, 1, 2.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)temp); CPhidget_delete((CPhidgetHandle)temp); //all done, exit return 0; }
int ph_simple() { int result; const char *err; //Declare an PH Sensor handle CPhidgetPHSensorHandle ph = 0; //create the PH Sensor object CPhidgetPHSensor_create(&ph); //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)ph, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)ph, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)ph, ErrorHandler, NULL); //Registers a callback that will run if the PH changes by more than the PH trigger. //Requires the handle for the PHSensor, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetPHSensor_set_OnPHChange_Handler(ph, PHChangeHandler, NULL); //open the PH Sensor for device connections CPhidget_open((CPhidgetHandle)ph, -1); //get the program to wait for an PH Sensor device to be attached printf("Waiting for PH Sensor to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)ph, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached textlcd device display_properties(ph); //read led event data printf("Reading.....\n"); //increase the sensitivity printf("Increasing sensitivity to 10.00, Press any key to continue\n"); getchar(); CPhidgetPHSensor_setPHChangeTrigger (ph, 10.00); //end 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)ph); CPhidget_delete((CPhidgetHandle)ph); //all done, exit return 0; }
int spatial_simple() { int result; const char *err; //Declare a spatial handle CPhidgetSpatialHandle spatial = 0; //create the spatial object CPhidgetSpatial_create(&spatial); //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)spatial, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL); //Registers a callback that will run according to the set data rate that will return the spatial data changes //Requires the handle for the Spatial, the callback handler function that will be called, //and an arbitrary pointer that will be supplied to the callback function (may be NULL) CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL); //open the spatial object for device connections CPhidget_open((CPhidgetHandle)spatial, -1); //get the program to wait for a spatial device to be attached printf("Waiting for spatial to be attached.... \n"); if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached spatial device display_properties((CPhidgetHandle)spatial); //read spatial event data printf("Reading.....\n"); //Set the data rate for the spatial events CPhidgetSpatial_setDataRate(spatial, 16); //run until user input is read 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)spatial); CPhidget_delete((CPhidgetHandle)spatial); return 0; }
int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_interface_kit"); ros::NodeHandle n; ros::NodeHandle nh("~"); int serial_number = -1; nh.getParam("serial", serial_number); std::string name = "interface_kit"; nh.getParam("name", name); if (serial_number==-1) { nh.getParam("serial_number", serial_number); } std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); double frequency = 30.0; nh.getParam("frequency", frequency); if (attach(phid, serial_number)) { display_properties(phid); const int buffer_length = 100; std::string topic_name = topic_path + name; std::string service_name = "interface_kit"; if (serial_number > -1) { char ser[10]; sprintf(ser,"%d", serial_number); topic_name += "/"; topic_name += ser; service_name += "/"; service_name += ser; } interface_kit_pub = n.advertise<phidgets::interface_kit_params>(topic_name, buffer_length); // start service which can be used to set digital outputs ros::ServiceServer service = n.advertiseService(service_name, set_values); initialised = true; ros::Rate loop_rate(frequency); while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } } return 0; }
int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_phsensor"); ros::NodeHandle n; ros::NodeHandle nh("~"); int serial_number = -1; nh.getParam("serial", serial_number); std::string name = "phsensor"; nh.getParam("name", name); if (serial_number==-1) { nh.getParam("serial_number", serial_number); } std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); double frequency = 30.0; nh.getParam("frequency", frequency); if (attach(phid, serial_number)) { display_properties(phid); const int buffer_length = 100; std::string topic_name = topic_path + name; if (serial_number > -1) { char ser[10]; sprintf(ser,"%d", serial_number); topic_name += "/"; topic_name += ser; } phsensor_pub = n.advertise<phidgets::phsensor_params>(topic_name, buffer_length); ros::Rate loop_rate(frequency); initialised = true; while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } disconnect(phid); } return 0; }
void dispaly_classes() { HashTable *class_tbl = CG(class_table); class_entry *ce = NULL; Bucket *p; st_output_debug_string("Classes:\n\n"); p = class_tbl->pListHead; while (p != NULL) { ce = p->pDataPtr; st_output_debug_string("class %s {\n", ce->name); st_output_debug_string(" Methods:\n"); display_functions(&ce->function_table); display_properties(&ce->properties_info); st_output_debug_string("}\n\n"); p = p->pListNext; } }
int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_led"); ros::NodeHandle n; ros::NodeHandle nh("~"); int serial_number = -1; nh.getParam("serial", serial_number); std::string name = "led"; nh.getParam("name", name); if (serial_number==-1) { nh.getParam("serial_number", serial_number); } std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); int frequency = 30; nh.getParam("frequency", frequency); if (attach(phid, serial_number)) { display_properties(phid); std::string topic_name = topic_path + name; if (serial_number > -1) { char ser[10]; sprintf(ser,"%d", serial_number); topic_name += "/"; topic_name += ser; } ros::Subscriber led_sub = n.subscribe(topic_name, 1, ledCallback); ros::Rate loop_rate(frequency); while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } disconnect(phid); } return 0; }
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; }
int IR_Simple() { CPhidgetIRHandle ir = 0; int result, i, dataLength, bitCount; const char *err; unsigned char data[16]; //Apple volume up unsigned char code[4] = {0x77,0xe1,0xd0,0xf0}; //Apple volume up int rawData[67] = { 9040, 4590, 540, 630, 550, 1740, 550, 1750, 550, 1740, 550, 620, 550, 1750, 550, 1740, 550, 1750, 550, 1740, 550, 1740, 560, 1740, 540, 630, 550, 620, 550, 620, 540, 630, 550, 1750, 550, 1740, 560, 1740, 550, 620, 550, 1740, 550, 620, 550, 620, 560, 610, 550, 620, 550, 1750, 550, 1740, 550, 620, 550, 1740, 550, 1750, 550, 620, 550, 620, 550, 620, 540}; //Apple uses standard NEC code CPhidgetIR_CodeInfo codeInfo = {0}; //this sets eveything to 0 - important if we're NOT going to explicitely fill everything in codeInfo.bitCount = 32; codeInfo.encoding = PHIDGET_IR_ENCODING_SPACE; codeInfo.gap = 110000; codeInfo.trail = 560; codeInfo.header[0] = 9000, codeInfo.header[1] = 4500; codeInfo.one[0] = 560, codeInfo.one[1] = 1700; codeInfo.zero[0] = 560, codeInfo.zero[1] = 560; codeInfo.repeat[0] = 9000, codeInfo.repeat[1] = 2250, codeInfo.repeat[2] = 560; //The rest of these parameters don't need to be filled in, as we're going to use the defaults //but this is how they could be filled in: //codeInfo.length = PHIDGET_IR_LENGTH_CONSTANT; //codeInfo.min_repeat = 1; ////toggle mask should be bit-length long //codeInfo.toggle_mask[0] = 0x00, codeInfo.toggle_mask[1] = 0x00, codeInfo.toggle_mask[2] = 0x00, codeInfo.toggle_mask[3] = 0x00; //codeInfo.carrierFrequency = 38000; //codeInfo.dutyCycle = 33; CPhidgetIR_create(&ir); CPhidget_set_OnAttach_Handler((CPhidgetHandle)ir, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)ir, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)ir, ErrorHandler, NULL); CPhidgetIR_set_OnCode_Handler(ir, CodeHandler, NULL); CPhidgetIR_set_OnLearn_Handler(ir, LearnHandler, NULL); CPhidgetIR_set_OnRawData_Handler(ir, RawDataHandler, NULL); CPhidget_open((CPhidgetHandle)ir, -1); printf("Waiting for PhidgetIR to be attached.... \n"); if((result = CPhidget_waitForAttachment((CPhidgetHandle)ir, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached accelerometer device display_properties((CPhidgetHandle)ir); printf("Reading.....\n"); printf("Press any key to Transmit a code...\n"); getchar(); if(result = CPhidgetIR_Transmit(ir, code, &codeInfo)) { CPhidget_getErrorDescription(result, &err); printf("Error: %s\n", err); } printf("Press any key to Transmit some raw data...\n"); getchar(); CPhidgetIR_TransmitRaw(ir, rawData, 67, 38000, 33, 110000); printf("Press any key to get the last code read...\n"); getchar(); dataLength = 16; if((result = CPhidgetIR_getLastCode(ir, data, &dataLength, &bitCount))) { CPhidget_getErrorDescription(result, &err); printf("Error: %s\n", err); } else { printf("Last Code: "); for(i = 0; i < dataLength; i++) { printf("%02x", data[i]); } printf("\n"); } printf("Press any key to end\n"); getchar(); printf("Closing...\n"); CPhidget_close((CPhidgetHandle)ir); CPhidget_delete((CPhidgetHandle)ir); return 0; }
int main(int argc, char* argv[]) { ros::init(argc, argv, "phidgets_motor_control_hc"); ros::NodeHandle n; ros::NodeHandle nh("~"); int serial_number = -1; nh.getParam("serial", serial_number); std::string name = "motorcontrol"; nh.getParam("name", name); nh.getParam("x_forward", x_forward); nh.getParam("rotation", rotation_offset); std::string odometry_topic = "odom"; nh.getParam("odometry", odometry_topic); double g = 0; const double min_g = 0.00001; nh.getParam("max_angular_error", g); if (g>min_g) max_angular_error = g; g=0; nh.getParam("max_velocity_error", g); if (g>min_g) max_velocity_error = g; g=0; nh.getParam("linear_deadband", g); if (g>min_g) linear_deadband = g; g=0; nh.getParam("angular_deadband", g); if (g>min_g) angular_deadband = g; g=0; nh.getParam("linear_proportional", g); if (g>min_g) linear_velocity_proportional = g; g=0; nh.getParam("linear_integral", g); if (g>min_g) linear_velocity_integral = g; g=0; nh.getParam("linear_derivative", g); if (g>min_g) linear_velocity_derivative = g; g=0; nh.getParam("angular_proportional", g); if (g>min_g) angular_velocity_proportional = g; g=0; nh.getParam("angular_integral", g); if (g>min_g) angular_velocity_integral = g; g=0; nh.getParam("angular_derivative", g); if (g>min_g) angular_velocity_derivative = g; g=0; nh.getParam("max_angular_velocity", g); if (g>min_g) max_angular_velocity = g; nh.getParam("invert_rotation", invert_rotation); nh.getParam("invert_forward", invert_forward); if (serial_number==-1) { nh.getParam("serial_number", serial_number); } g=0; nh.getParam("max_angular_accel", g); if (g>0) max_angular_accel = g; g=0; nh.getParam("max_linear_accel", g); if (g>0) max_linear_accel = g; std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); int timeout_sec = 2; nh.getParam("timeout", timeout_sec); int v=0; nh.getParam("speed", v); if (v>0) speed = v; int frequency = 30; nh.getParam("frequency", frequency); ITerm[0]=0; ITerm[1]=0; if (attach(phid, serial_number)) { display_properties(phid); const int buffer_length = 100; std::string topic_name = topic_path + name; std::string service_name = name; if (serial_number > -1) { char ser[10]; sprintf(ser,"%d", serial_number); topic_name += "/"; topic_name += ser; service_name += "/"; service_name += ser; } motors_pub = n.advertise<ros_phidgets_jade::motor_params>(topic_name, buffer_length); // receive velocity commands ros::Subscriber command_velocity_sub = n.subscribe("cmd_vel", 1, velocityCommandCallback); // subscribe to odometry ros::Subscriber odometry_sub = n.subscribe(odometry_topic, 1, odometryCallback); initialised = true; ros::Rate loop_rate(frequency); while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); // SAFETY FEATURE // if a velocity command has not been received // for a period of time then stop the motors double time_since_last_command_sec = (ros::Time::now() - last_velocity_command).toSec(); if ((motors_active) && (time_since_last_command_sec > timeout_sec)) { stop_motors(); ROS_WARN("No velocity command received - " \ "motors stopped"); } } disconnect(phid); } return 0; }
int LED_simple() { int result, i; const char *err; //Declare an LED handle CPhidgetLEDHandle led = 0; //create the LED object CPhidgetLED_create(&led); //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)led, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)led, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)led, ErrorHandler, NULL); //open the LED for device connections CPhidget_open((CPhidgetHandle)led, -1); //get the program to wait for an LED device to be attached printf("Waiting for LED to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)led, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached LED device display_properties(led); printf("Press any key to continue\n"); getchar(); //turn on the leds one at a time. //This example assumes LED's plugged into locations 0-9 for(i = 0; i < 10; i++) { CPhidgetLED_setBrightness(led, i, 100); //maximum brightness is 100, 0 is off. Can set this value to anything including and inbetween these values. } printf("Press any key to continue\n"); getchar(); //turn off the LEDs one at a a time //This example assumes LED's plugged into locations 0-9 for(i = 0; i < 10; i++) { CPhidgetLED_setBrightness(led, i, 0); //maximum brightness is 100, 0 is off. Can set this value to anything including and inbetween these values. } 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)led); CPhidget_delete((CPhidgetHandle)led); //all done, exit return 0; }
int rfid_simple() { int result; const char *err; //Declare an RFID handle CPhidgetRFIDHandle rfid = 0; printf("RFIDHandler\n"); //create the RFID object CPhidgetRFID_create(&rfid); printf("Create\n"); //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)rfid, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)rfid, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)rfid, ErrorHandler, NULL); printf("device EVENT Handlers\n"); //Registers a callback that will run if an output changes. //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). CPhidgetRFID_set_OnOutputChange_Handler(rfid, OutputChangeHandler, NULL); //Registers a callback that will run when a Tag is read. //Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetRFID_set_OnTag_Handler(rfid, TagHandler, NULL); //Registers a callback that will run when a Tag is lost (removed from antenna read range). //Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetRFID_set_OnTagLost_Handler(rfid, TagLostHandler, NULL); printf("RFID EVENT Handlers\n"); //open the RFID for device connections CPhidget_open((CPhidgetHandle)rfid, -1); printf("OPENED\n"); sleep(1); //get the program to wait for an RFID device to be attached printf("Waiting for RFID to be attached....\n"); if((result = CPhidget_waitForAttachment((CPhidgetHandle)rfid, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached RFID device display_properties(rfid); CPhidgetRFID_setAntennaOn(rfid, 1); //read RFID event data printf("Reading.....\n"); //keep displaying RFID event data until user input is read printf("Press any key to continue\n"); getchar(); //toggle the digital output (when making this example I had an LED plugged into the digital output index 0 CPhidgetRFID_setOutputState(rfid, 0, 1); //keep displaying RFID event data until user input is read printf("Press any key to continue\n"); getchar(); //toggle the digital output (when making this example I had an LED plugged into the digital output index 0 CPhidgetRFID_setOutputState(rfid, 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)rfid); CPhidget_delete((CPhidgetHandle)rfid); //all done, exit return 0; }
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; }
int accelerometer_simple() { int result, numAxes; const char *err; //Declare an accelerometer handle CPhidgetAccelerometerHandle accel = 0; //create the accelerometer object CPhidgetAccelerometer_create(&accel); //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)accel, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)accel, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)accel, ErrorHandler, NULL); //Registers a callback that will run if the acceleration changes by more than the Acceleration trigger. //Requires the handle for the Accelerometer, the function that will be called, //and an arbitrary pointer that will be supplied to the callback function (may be NULL) CPhidgetAccelerometer_set_OnAccelerationChange_Handler(accel, accel_AccelChangeHandler, NULL); //open the acclerometer for device connections CPhidget_open((CPhidgetHandle)accel, -1); //get the program to wait for an accelerometer device to be attached printf("Waiting for accelerometer to be attached.... \n"); if((result = CPhidget_waitForAttachment((CPhidgetHandle)accel, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached accelerometer device display_properties((CPhidgetHandle)accel); //read accelerometer event data printf("Reading.....\n"); //get the number of available axes on the attached accelerometer CPhidgetAccelerometer_getAxisCount(accel, &numAxes); //Most accelerometers have 2 axes so we'll pre-set their sensitivity to make the data more readable CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 0, 0.500); CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 1, 0.500); //If the accelerometer attached is a 3-axis, we'll set the sensitivity of the 3rd axis if(numAxes > 2) { CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 2, 0.500); } //wait until user input is read 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)accel); CPhidget_delete((CPhidgetHandle)accel); //all done, exit 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; }
int interfacekit_simple() { int result, numSensors, i; const char *err; //Declare an InterfaceKit handle CPhidgetInterfaceKitHandle 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); //Registers a callback that will run if an input changes. //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). CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL); //Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger. //Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL); //Registers a callback that will run if an output changes. //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). CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL); //open the interfacekit for device connections CPhidget_open((CPhidgetHandle)ifKit, -1); //get the program to wait for an interface kit device to be attached printf("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; } int count = 0; bool run = true; power_button_reset(); while (run) { printf("Hello\n"); int lw = whleft.state(); int rw = whright.state(); int he = hall.state(); int sn = sonar.state(); if(power_button_get_value() >= 2){ power_button_reset(); } if(he == 1 && motor.moveState() == 1){ count +=1; } if(he == 0){ count = 0; } if(power_button_get_value() < 1){ motor.stop(); count = 0; }else if (sn == 1) { motor.right(); }else if(lw == 1) { motor.right(); }else if(rw == 1) { motor.left(); }else if(he == 1 && motor.moveState() == 1 && count>5) { motor.reverse(); sleep(5); motor.right(); sleep(3); count =0; }else{ motor.forwards(); } printf("Left Whisker: %d, Right Whisker: %d, Hall Effect: %d, Sonar: %d\n", lw, rw, he, sn); sleep(1); } //Display the properties of the attached interface kit device display_properties(ifKit); //read interface kit event data /*printf("Reading.....\n"); //keep displaying interface kit data until user input is read printf("Press any key to go to next step\n"); getchar(); printf("Modifying sensor sensitivity triggers....\n"); //get the number of sensors available CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors); //Change the sensitivity trigger of the sensors for(i = 0; i < numSensors; i++) { CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100); //we'll just use 10 for fun } //read interface kit event data printf("Reading.....\n"); //keep displaying interface kit data until user input is read printf("Press any key to go to next step\n"); getchar(); printf("Toggling Ratiometric....\n"); CPhidgetInterfaceKit_setRatiometric(ifKit, 0); //read interface kit event data printf("Reading.....\n");*/ //keep displaying interface kit data until user input is read 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)ifKit); CPhidget_delete((CPhidgetHandle)ifKit); //all done, exit return 0; }
int interfacekit_simple() { int result, numSensors, i; const char *err; //Declare an InterfaceKit handle CPhidgetInterfaceKitHandle 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); //Registers a callback that will run if an input changes. //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). CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL); //Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger. //Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL). CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL); //Registers a callback that will run if an output changes. //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). CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL); //open the interfacekit for device connections CPhidget_open((CPhidgetHandle)ifKit, -1); //get the program to wait for an interface kit device to be attached printf("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 display_properties(ifKit); //read interface kit event data printf("Reading.....\n"); //keep displaying interface kit data until user input is read printf("Press any key to go to next step\n"); getchar(); printf("Modifying sensor sensitivity triggers....\n"); //get the number of sensors available CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors); //Change the sensitivity trigger of the sensors for(i = 0; i < numSensors; i++) { CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100); //we'll just use 10 for fun } //read interface kit event data printf("Reading.....\n"); //keep displaying interface kit data until user input is read printf("Press any key to go to next step\n"); getchar(); printf("Toggling Ratiometric....\n"); CPhidgetInterfaceKit_setRatiometric(ifKit, 0); //read interface kit event data printf("Reading.....\n"); //keep displaying interface kit data until user input is read 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)ifKit); CPhidget_delete((CPhidgetHandle)ifKit); //all done, exit return 0; }
int textlcd_simple() { int result; const char *err; //Declare an TextLCD handle CPhidgetTextLCDHandle txt_lcd = 0; //create the TextLCD object CPhidgetTextLCD_create(&txt_lcd); //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)txt_lcd, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)txt_lcd, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)txt_lcd, ErrorHandler, NULL); //open the TextLCD for device connections CPhidget_open((CPhidgetHandle)txt_lcd, -1); //get the program to wait for an TextLCD device to be attached printf("Waiting for TextLCD to be attached....\n"); if((result = CPhidget_waitForAttachment((CPhidgetHandle)txt_lcd, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached textlcd device display_properties(txt_lcd); //read TextLCD event data printf("Reading.....\n"); //Begin simulation of capabilities //Step 1: Write a simple message to the first row printf("Writing to first row. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setDisplayString (txt_lcd, 0, "Row 1"); //Step 2: write a simple message to the second row printf("Writing to second row. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setDisplayString (txt_lcd, 1, "Row 2"); //Step 3: turn up, turn down, and set back to default the contrast printf("Adjusting contrast up. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setContrast (txt_lcd, 255); //valid range is 0 - 255, default is 0 normal viewable seems to be around 100 printf("Restoring default contrast. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setContrast (txt_lcd, 110); //Step 4: Turn on the cursor printf("Turn on cursor. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setCursorOn (txt_lcd, 1); //Step 5: turn on the cursor blink printf("Turn on cursor blink. Press any key to continue\n"); getchar(); CPhidgetTextLCD_setCursorOn (txt_lcd, 0); CPhidgetTextLCD_setCursorBlink (txt_lcd, 1); //End simulation printf("Press any key to end\n"); getchar(); CPhidgetTextLCD_setCursorBlink (txt_lcd, 0); CPhidgetTextLCD_setDisplayString (txt_lcd, 0, ""); CPhidgetTextLCD_setDisplayString (txt_lcd, 1, ""); //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)txt_lcd); CPhidget_delete((CPhidgetHandle)txt_lcd); //all done, exit return 0; }