int teardown() { SetupLog("Closing...\n"); #ifndef NO_POWERLIB power_button_reset(); #endif CPhidgetAdvancedServo_setPosition(servo, 0, 120); sleep(2); // Close IFKIT CPhidget_close((CPhidgetHandle)ifKit); CPhidget_delete((CPhidgetHandle)ifKit); // close motoControl CPhidget_close((CPhidgetHandle)motoControl); CPhidget_delete((CPhidgetHandle)motoControl); // disengage and close servo CPhidgetAdvancedServo_setEngaged(servo, 0, 0); CPhidget_close((CPhidgetHandle)servo); CPhidget_delete((CPhidgetHandle)servo); //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 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; }