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 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 main(int argc, char *argv[])
{
Sensing* sensing = new Sensing();
Control* control = new Control();
ServoControl* servo = new ServoControl();
servo->setEventHandler(sensing);
Strategy* strategies [] = {
new FindSiteStrategy(sensing, control, servo), new HitButtonStrategy(sensing, control, servo),
new HalfHZDanceStrategy(sensing, control, servo), new OneHZDanceStrategy(sensing, control, servo),
new TwoHZDanceStrategy(sensing, control, servo), new FourHZDanceStrategy(sensing, control, servo),
new SixHZDanceStrategy(sensing, control, servo), new EightHZDanceStrategy(sensing, control, servo)
};
int STRATEGIES_COUNT = 8;
servo->setPosition(90);
printf("READY!\n");
int pwr = power_button_get_value();
while(pwr == power_button_get_value()) {
msleep(400);
}
timespec current;
timespec old;
clock_gettime(CLOCK_MONOTONIC, ¤t);
pwr = power_button_get_value();
Strategy* oldStrategy = 0;
while (pwr == power_button_get_value()) {
printf("-------------------------------\n");
double bestUtility = -1;
int strategyIndex = 0;
Strategy* strategy = 0;
for (int i = 0; i < STRATEGIES_COUNT; i++) {
Strategy* s = strategies[i];
double util = s->getUtility();
if (util > bestUtility) {
strategy = s;
bestUtility = util;
strategyIndex = i;
}
}
//printf("B: %d\n", sensing->getBottomDistance());
//printf("T: %d\n", sensing->getTopDistance());
old = current;
clock_gettime(CLOCK_MONOTONIC, ¤t);
double diff = (current.tv_sec - old.tv_sec) + ((current.tv_nsec - old.tv_nsec) / NANOSECONDS_PER_SECOND);
printf("T: %f, S: %d\n", diff, strategyIndex);
control->controlTick(diff);
strategy->step(diff, strategy != oldStrategy);
oldStrategy = strategy;
//double f = sensing->getFrequency();
printf("DISTANCES: top %d, bottom %d, sonar %d\n", sensing->getTopDistance(), sensing->getBottomDistance(), sensing->getSonarDistance(0));
msleep(90);
}
control->stop();
msleep(100);
for (int i = 0; i < STRATEGIES_COUNT; i++) {
delete strategies[i];
}
servo->setEventHandler(NULL);
delete servo;
delete control;
delete sensing;
power_button_reset();
return 0;
}
