void Elevator::Stack_internal()
{
switch (m_stackCase)
{
case 0:
Set(kBottom);
if (AtSetpoint())
{
m_stackCase++;
m_stackTime->Stop();
m_stackTime->Start();
m_stackTime->Reset();
}
break;
case 1:
if (m_stackTime->HasPeriodPassed(.1))
{
m_stackCase++;
m_stackTime->Stop();
m_stackTime->Reset();
}
break;
case 2:
Set(m_stackFin);
if (AtSetpoint())
{
m_stackCase++;
}
break;
}
}
void Elevator::PrintData()
{
if (f_setPID)
m_pid->SetPID(SmartDashboard::GetNumber("Elevator P"),SmartDashboard::GetNumber("Elevator I"),SmartDashboard::GetNumber("Elevator D"));
else
{
SmartDashboard::PutNumber("Left Elevator",m_lElevator->Get());
SmartDashboard::PutNumber("Right Elevator",m_rElevator->Get());
SmartDashboard::PutNumber("Elevator Distance",m_elevEncode->GetDistance());
SmartDashboard::PutNumber("Elevator Rate",m_elevEncode->GetRate());
SmartDashboard::PutNumber("Elevator P",m_pid->GetP());
SmartDashboard::PutNumber("Elevator I",m_pid->GetI());
SmartDashboard::PutNumber("Elevator D",m_pid->GetD());
SmartDashboard::PutNumber("Elevator PID Output",m_pid->Get());
SmartDashboard::PutNumber("Elevator PID Is Enabled",m_pid->IsEnabled());
SmartDashboard::PutBoolean("Elevator At Setpoint",AtSetpoint());
SmartDashboard::PutNumber("Elevator Difference",fabs(m_elevEncode->GetDistance() - m_pid->GetSetpoint()));
SmartDashboard::PutNumber("Elevator Stack Case",m_stackCase);
SmartDashboard::PutNumber("Elevator Stack Finish",GetPosition(m_stackFin));
SmartDashboard::PutBoolean("Stacking",Stacking());
}
}
void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (running)
{
enterHoldCommand = joystick->GetRawButton(BUT_JS_ENT_POS_HOLD);
exitHoldCommand = joystick->GetRawButton(BUT_JS_EXIT_POS_HOLD);
switch (liftState)
{
case raising:
if (GetLiftLimitSwitchMax())
{
SetLiftMotor(MOTOR_SPEED_STOP);
if(!liftEncFullRanged)
{
maxLiftEncDist = liftEncoder->GetDistance();
liftEncFullRanged = true;
}
motorSpeed = -MOTOR_SPEED_DOWN;
liftState = lowering;
SetLiftMotor(motorSpeed);
}
if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
{
liftState = holding;
}
break;
case lowering:
if (GetLiftLimitSwitchMin())
{
SetLiftMotor(MOTOR_SPEED_STOP);
if(!liftEncZeroed)
{
liftEncoder->Reset();
liftEncZeroed = true;
}
motorSpeed=MOTOR_SPEED_UP;
liftState = raising;
SetLiftMotor(motorSpeed);
}
if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
{
liftState = holding;
}
break;
case holding:
if(!(controlLift->IsEnabled()))
{
pidPosSetPoint = SP_RANGE_FRACTION*maxLiftEncDist; //go to the midpoint of the range
controlLift->SetSetpoint(pidPosSetPoint);
#if BUILD_VERSION == COMPETITION
controlLift2->SetSetpoint(pidPosSetPoint);
#endif
controlLift->Enable();
#if BUILD_VERSION == COMPETITION
controlLift2->Enable();
#endif
}
if(exitHoldCommand)
{
controlLift->Disable();
#if BUILD_VERSION == COMPETITION
controlLift2->Disable();
#endif
motorSpeed = -MOTOR_SPEED_DOWN;
liftState = lowering;
SetLiftMotor(motorSpeed);
}
break;
}
}
//status
sprintf(myString, "running: %d\n", running);
SmartDashboard::PutString("DB/String 0", myString);
sprintf(myString, "State: %d\n", liftState);
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "motorSpeed: %f\n", motorSpeed);
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "lift encoder zeroed: %d\n", liftEncZeroed);
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "max enc set: %d\n", liftEncFullRanged);
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "maxLiftEncDist: %f\n", maxLiftEncDist);
SmartDashboard::PutString("DB/String 5", myString);
sprintf(myString, "enc dist: %f\n", liftEncoder->GetDistance());
SmartDashboard::PutString("DB/String 6", myString);
sprintf(myString, "pid: %d\n", controlLift->IsEnabled());
SmartDashboard::PutString("DB/String 7", myString);
sprintf(myString, "dist to sp : %f\n", DistToSetpoint());
SmartDashboard::PutString("DB/String 8", myString);
sprintf(myString, "at sp : %d\n", AtSetpoint());
SmartDashboard::PutString("DB/String 9", myString);
}
bool Elevator::AtSetpoint()
{
return AtSetpoint(0.03);
}
