void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
GetWatchdog().Feed();
// if(autoPilot == true)
// {
// Auto Align Disable Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
// {
// Goal_Align_PID->Disable(); // Stop outputs
// Goal_Align_PID->Enable(); // Start PIDContoller up again
// Goal_Align_PID->SetSetpoint(0.0);
// autoPilot = false;
// }
// }
// else
// {
// Calculate jaguar output based on exponent we pass from SmartDashboard
double leftOutput, rightOutput;
leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
{
printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
}
buttonWasDown = operatorGamepad->GetRawButton(1);
// Auto Align Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
// {
// // Turn Auto Align on if we see a goal and we know the azimuth
// if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
// {
// Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
// autoPilot = true;
// }
// }
// }
}
float RawControl::getArmPos()
{
return arm->GetPosition();
}
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier indirectly through CallCalculate
* and is created during initialization.
*/
void CAN_VPID_Controller::Calculate()
{
bool enabled;
CANJaguar *pidInput;
CRITICAL_REGION(m_semaphore)
{
if (m_pidInput == 0) return;
if (m_pidOutput1 == 0) return;
if (m_pidOutput2 == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
END_REGION;
if (enabled)
{
float currentPosition = pidInput->GetPosition();
float input = (currentPosition-m_prevPosition);
float result;
//printf("Target - %3.5f, Current - %3.5f\n\r", m_setpoint, currentPosition - m_prevPosition);
PIDOutput *pidOutput1, *pidOutput2;
{
Synchronized sync(m_semaphore);
m_error = m_setpoint - input;
if (m_continuous)
{
if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
{
if (m_error > 0)
{
m_error = m_error - m_maximumInput + m_minimumInput;
}
else
{
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput)
{
if (potentialIGain > m_minimumOutput)
m_totalError += m_error;
else
m_totalError = m_minimumOutput / m_I;
}
else
{
m_totalError = m_maximumOutput / m_I;
}
m_result = m_P * m_totalError + m_I * m_error + m_D * (m_error - m_prevError) + m_F * m_setpoint;
m_prevError = m_error;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
pidOutput1 = m_pidOutput1;
pidOutput2 = m_pidOutput2;
result = .3*m_result+.7*m_previousResult;
m_previousResult = result;
}
pidOutput1->PIDWrite(result);
pidOutput2->PIDWrite((m_InvertOutputs) ? -result : result);
m_prevPosition = currentPosition; // Store the current position
}
}