void ManualMainLoop(void) { // Set up the turret control IP. InitMotorBoard(); TurnOnLaser(); EnablePanServo(); EnableTiltServo(); SetTiltAngle(0); SetPanAngle(0); continueTest = true; xil_printf("Entering manual mode.\r\n"); while(continueTest) { // We allow users to press more than one button at a time. if (sButtonState & BUTTON_LEFT) { PanLeft(1); } if (sButtonState & BUTTON_RIGHT) { PanRight(1); } if (sButtonState & BUTTON_UP) { TiltUp(1); } if (sButtonState & BUTTON_DOWN) { TiltDown(1); } MB_Sleep(7); } SetTiltAngle(0); SetPanAngle(0); MB_Sleep(100); DisableTiltServo(); DisablePanServo(); TurnOffLaser(); }
bool OGLObjsCamera::HandleKeystroke( const unsigned char in_c, const bool in_bShift, const bool in_bCntrl ) { switch ( in_c ) { case 'r' : if ( in_bShift == TRUE ) SpinClockwise( s_dDeltaRot ); else SpinCounterClockwise( s_dDeltaRot ); break; case 'x': if ( in_bShift == TRUE ) PosXAxis( ); else NegXAxis( ); break; case 'y': if ( in_bShift == TRUE ) PosYAxis( ); else NegYAxis( ); break; case 'z': if ( in_bShift == TRUE ) PosZAxis( ); else NegZAxis( ); break; case '1': RotateSelf(0, ((in_bShift == TRUE) ? -1.0 : 1.0) * s_dDeltaRot); break; case '2': RotateSelf(1, ((in_bShift == TRUE) ? -1.0 : 1.0) * s_dDeltaRot); break; case '3': RotateSelf(2, ((in_bShift == TRUE) ? -1.0 : 1.0) * s_dDeltaRot); break; case FL_Home: // up arrow Reset(); break; case 'h': // up arrow case FL_Page_Up : if ( in_bShift == TRUE ) { PanIn( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaTrans *= 2.0; } else { SpinClockwise( s_dDeltaRot ); } break; case 'l': // up arrow case FL_Page_Down : if ( in_bShift == TRUE ) { PanOut( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaTrans *= 2.0; } else { SpinCounterClockwise( s_dDeltaRot ); } break; case 'i': // up arrow case FL_Up : if ( in_bShift == TRUE ) { PanUp( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaTrans *= 2.0; } else { RotateUp( s_dDeltaRot ); } break; case 'm': // down arrow case FL_Down : if ( in_bShift == TRUE ) { PanDown( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaTrans *= 0.5; } else { RotateDown( s_dDeltaRot ); } break; case 'j': // left arrow case FL_Left : if ( in_bShift == TRUE ) { PanLeft( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaRot *= 0.5; } else { RotateLeft( s_dDeltaRot ); } break; case 'k': // right arrow case FL_Right : if ( in_bShift == TRUE ) { PanRight( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaRot *= 2.0; } else { RotateRight( s_dDeltaRot ); } break; case 'p' : // zoom if ( in_bShift == TRUE ) { PanIn( s_dDeltaTrans ); } else if ( in_bCntrl == TRUE ) { s_dDeltaZoom *= 1.1; } else { SetZoom( s_dDeltaZoom * GetZoom() ); } break; case 'n' : if ( in_bShift == TRUE ) { PanOut( s_dDeltaZoom ); } else if ( in_bCntrl == TRUE ) { s_dDeltaZoom *= 0.9; } else { SetZoom( GetZoom() / s_dDeltaZoom ); } break; default: //TRACE("Unknown char %d\n", Key); return FALSE; } return TRUE; }
static void AutoMainLoop(void) { int x_diff; int y_diff; int x_adj; int y_adj; InitMotorBoard(); TurnOnLaser(); EnablePanServo(); EnableTiltServo(); SetTiltAngle(DEFAULT_X_POS); SetPanAngle(DEFAULT_Y_POS); int acquired = 0; int sleep_interval; continueTest = true; xil_printf("Entering auto mode.\r\n"); while(continueTest) { runImageProcessing(); SetObjIdValue(1); TargetingState state = get_targeting_state(); int target_x = state.target_loc.x; int target_y = state.target_loc.y; u32 target_size = state.target_size; xil_printf( "Targeting state: Laser = (%d,%d); Obj = (%d, %d) [sz=%d]\n\r", state.laser.x, state.laser.y, target_x, target_y, target_size); if (state.laser.x == 0 && state.laser.y == 0) { SetPanAngle(DEFAULT_X_POS); SetTiltAngle(DEFAULT_Y_POS); playNeg(); goto loop_sleep; } // Get the diffs in both dimensions. //x_diff = X_MIDDLE; //y_diff = Y_MIDDLE; x_diff = (target_x - state.laser.x); y_diff = (state.laser.y - target_y); // Determine how much to adjust the angles. x_adj = (x_diff > 0) ? x_diff : -x_diff; x_adj /= 16; int MAX_ANGLE = 10; if (x_adj > MAX_ANGLE) { x_adj = MAX_ANGLE; } else if (x_adj == 0 && x_diff != 0) { x_adj = 1; } y_adj = (y_diff > 0) ? y_diff : -y_diff; y_adj /= 16; if (y_adj > MAX_ANGLE) { y_adj = MAX_ANGLE; } else if (y_adj == 0 && y_diff != 0) { y_adj = 1; } // Adjust laser position to correct for these diffs. if (x_diff > LASER_TOLERANCE) { xil_printf("Need to move laser right by %d deg\r\n", x_adj); PanRight(x_adj); } else if (x_diff < -LASER_TOLERANCE) { xil_printf("Need to move laser left by %d deg\r\n", x_adj); PanLeft(x_adj); } else { xil_printf("X location on target!\r\n"); } if (y_diff > LASER_TOLERANCE) { xil_printf("Need to move laser up by %d deg\r\n", y_adj); TiltUp(y_adj); } else if (y_diff < -LASER_TOLERANCE) { xil_printf("Need to move laser down by %d deg\r\n", y_adj); TiltDown(y_adj); } else { xil_printf("Y location on target!\r\n"); } if (x_diff <= LASER_TOLERANCE && x_diff >= -LASER_TOLERANCE && y_diff <= LASER_TOLERANCE && y_diff >= -LASER_TOLERANCE) { if (acquired) { playPortalGunSound(); } else { playPos(); acquired = 1; } } else { acquired = 0; } loop_sleep: sleep_interval = sDebugOutputEnabled ? 3000 : 500; MB_Sleep(sleep_interval); } SetTiltAngle(0); SetPanAngle(0); MB_Sleep(100); DisableTiltServo(); DisablePanServo(); TurnOffLaser(); }