/*
double Autonomous::getTime()
{
}
*/
void Autonomous::updateHighGoal()
{
static int output=0;
switch (stage)
{
case IDLE:
printf("AUTO switch to DRIVE_AIM_WINCH\n");
stage = DRIVE_AIM_WINCH;
return;
case DRIVE_AIM_WINCH:
bool driveDone=moveForward(DISTANCE);
bool aimDone=tilt(HIGHGOAL_AUTOANGLE);
bool winchDone=wormPull();
if(output%20==0)
printf("drive: %i, aim: %i, winch: %i\n",driveDone,aimDone,winchDone);
if(driveDone && aimDone && winchDone)
{
printf("AUTO switch to SMART_FIRE\n");
stage = SMART_FIRE;
return;
}
break;
case SMART_FIRE:
if(smartFire())
{
printf("AUTO done\n");
stage = DONE;
return;
}
break;
case DONE:
robot->drive->TankDrive(0.0,0.0);
break;
default:
break;
}
previousStage = stage;
output++;
}
void Shooter::update()
{
double bobX = bobTheAccelerometer->GetAcceleration(ADXL345_I2C::kAxis_X);
double bobY = bobTheAccelerometer->GetAcceleration(ADXL345_I2C::kAxis_Y);
double bobZ = bobTheAccelerometer->GetAcceleration(ADXL345_I2C::kAxis_Z);
currentPitch = (atan2(bobX, sqrt(bobY*bobY + bobZ*bobZ))*180.0)/PI;
// manual controls
if(shooterJoy -> IsAxisZero(TILT))
{
pitchStop();
}
else if(shooterJoy -> GetRawAxis(TILT) < 0) // push up = negative values = tilt down
{
pitchDown();
}
else
{
pitchUp();
}
if(shooterJoy -> GetSmoothButton(ROLLERS))
{
pull();
}
else
{
pullStop();
}
if(shooterJoy -> GetTriggerState() == LOCKANDLOAD)
{
autoPulling = true;
wormPull();
}
else if(autoPulling)
{
autoPulling = false;
wormStop();
punch();
}
// auto pitch angle
if (isPitchingUp)
{
if (currentPitch <= destinationPitch || !(axis->GetForwardLimitOK()))
{
pitchStop();
isPitchingUp = false;
}
}
if (isPitchingDown)
{
if (currentPitch >= destinationPitch || !(axis->GetReverseLimitOK()))
{
pitchStop();
isPitchingDown = false;
}
}
// smart pickup control
// sequence:
// clamp up, tilt down
// when tilt at pickup position, clamp down, rollers pull
// when let go, tilt up
// when tilt at shoot position, clamp up, rollers stop
if(shooterJoy -> GetSmoothButton(PICKUP)) // holding down button
{
if (!isPickingUp) // hasn't started sequence yet
{
isPickingUp = true;
pitchAngle(PICKUP_POSITION);
clampUp();
}
if(!isPitchingDown) // tilt at pickup position
{
clampDown();
pull();
}
}
else // let go of button
{
if(isPickingUp) // was picking up, so now should stop picking up
{
pitchAngle(SHOOTING_POSITION);
isPickingUp = false;
isPickingUpStopping = true;
}
if (!isPitchingUp && isPickingUpStopping) // tilt at shooting position
{
clampUp();
pullStop();
isPickingUpStopping = false;
}
}
// increasing worm drive speed
if(wormIsPulling)
{
if(currentSpeed <= WORM_LIMIT)
{
currentSpeed += INCREMENT;
wormGear->Set(currentSpeed);
}
else if(currentSpeed > WORM_LIMIT || !(wormGear->GetForwardLimitOK()))
{
wormStop();
}
}
}