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();
}
