void ArmClass::HandleTarget(float centerX,float centerY,float calX,float calY)
{
if(fabs (centerX) >= 1 || fabs(centerY) >= 1)
{
LastMoveByDegreesX = LastMoveByDegreesY = 360.0f;
if (CurrentEnableTracking)
{
SetTurret(GetTurretEncoder());
SetArm(GetLifterEncoder());
}
return;
}
else
{
float moveByX_Degrees = 0;
float moveByY_Degrees = 0;
float moveByX_Ticks = 0;
float moveByY_Ticks = 0;
float xFOV = 76.00f;
float yFOV = 61.2f;
float ticksPerDegreeX = 1280/90.0f;
float ticksPerDegreeY = 5000/90.0f;
moveByX_Degrees = (calX - centerX) * (xFOV*.5f);
moveByY_Degrees = (calY - centerY) * (yFOV*.5f);
LastMoveByDegreesX = moveByX_Degrees;
LastMoveByDegreesY = moveByY_Degrees;
moveByX_Ticks = moveByX_Degrees * ticksPerDegreeX;
moveByY_Ticks = moveByY_Degrees * ticksPerDegreeY;
if(GetLifterEncoder() >= 0)
{
moveByX_Ticks *=-1.0f;
moveByY_Ticks *=-1.0f;
}
if (CurrentEnableTracking)
{
SetTurret(GetTurretEncoder()-(moveByX_Ticks* 1.0f)); //.85
SetArm(GetLifterEncoder()-(moveByY_Ticks* 1.0f));
}
}
}
void ArmClass::Auto_Start()
{
CurrentEnableTracking = false;
PreviousEnableTracking = false;
LastMoveByDegreesX = 360.0f;
LastMoveByDegreesY = 360.0f;
ResetEncoderLifter();
ResetEncoderTurret();
ArmPIDController->Disable();
ArmPIDController->Reset();
TurretPIDController->Disable();
TurretPIDController->Reset();
SetTurret(GetTurretEncoder());
SetArm(GetLifterEncoder());
isShooting = false;
ShotStage = 0;
ShotTimer->Reset();
ArmLockonTimer->Reset();
ArmLockonTimer->Start();
LastShotTimer->Reset();
LastShotTimer->Start();
}
void Shooter::Update()
{
double rotation = turretEncoder->GetDistance();
if( (rotation < -90 && turretDirection < 0) || (rotation > 90 && turretDirection > 0) )
SetTurret(0);
topEncoder->PIDGet();
bottomEncoder->PIDGet();
SHOOTER.secondaryDisplay.PrintfLine(0, "TopRate:%.3f", SHOOTER.topEncoder->GetRate());
SHOOTER.secondaryDisplay.PrintfLine(1, "BotRate:%.3f", SHOOTER.bottomEncoder->GetRate());
}
void ArmClass::ResetTurret()
{
SetTurret(0);
}
