public static void teleopPeriodic() {
/*
double newArmPos = gamepad.getRawAxis(1);
if(Math.abs(newArmPos) <= ARM_DEADZONE) {
newArmPos = 0.0;
}
double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER) + frontArmMotor.getPosition();
//double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER);
frontArmMotor.set(newMotorPos);
System.out.println("input = " + newArmPos + " target pos = " + newMotorPos + " enc pos = " + frontArmMotor.getPosition());
*/
// PercentVbus test ONLY!!
double armSpeed = gamepad.getRawAxis(1);
if (Math.abs(armSpeed) < ARM_DEADZONE) {
armSpeed = 0.0f;
}
armSpeed *= ARM_MULTIPLIER;
double pos = frontArmMotor.getPosition();
if (((pos > SOFT_ENCODER_LIMIT_1) && armSpeed < 0.0)
|| ((pos < SOFT_ENCODER_LIMIT_2) && armSpeed > 0.0)) armSpeed = 0.0;
frontArmMotor.set(armSpeed);
System.out.println("armSpeed = " + armSpeed + " enc pos = " + frontArmMotor.getPosition());
// check for roller motion (right gamepad joystick)
double rollerSpeed = gamepad.getRawAxis(3);
if (Math.abs(rollerSpeed) < ROLLER_DEADZONE) {
rollerSpeed = 0.0f;
}
rollerSpeed *= ARM_ROLLER_MULTIPLIER;
frontArmRollerMotor.set(rollerSpeed);
}
/**
* Set the setpoint for the PIDController
*
* @param setpoint the desired setpoint
*/
public synchronized void setSetpoint(double setpoint) {
m_setpoint = setpoint;
m_talon.set(setpoint);
;
reportPosition(m_talon.getPosition());
if (m_debugging) SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
}
private void initSmartDashboard() {
SmartDashboard.putNumber(m_name + " P", m_talon.getP());
SmartDashboard.putNumber(m_name + " I", m_talon.getI());
SmartDashboard.putNumber(m_name + " D", m_talon.getD());
SmartDashboard.putNumber(m_name + " F", m_talon.getF());
SmartDashboard.putNumber(m_name + " I*100", (m_talon.getI()) * 100);
SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
SmartDashboard.putNumber(m_name + " Position", m_talon.getPosition());
SmartDashboard.putNumber(m_name + " Tolerance", m_tolerance);
SmartDashboard.putNumber(m_name + " Error", m_talon.getClosedLoopError());
SmartDashboard.putNumber(m_name + " Output", m_talon.get());
SmartDashboard.putBoolean(m_name + " Enabled", m_talon.isControlEnabled());
}
public double getRightRawDistance() {
return rightMotor1.getPosition();
}
public double getLeftRawDistance() {
return leftMotor1.getPosition();
}
public synchronized boolean onTarget() {
return (Math.abs(getSetpoint() - m_talon.getPosition()) <= m_tolerance);
}
/**
* Returns the current position of the associated encoder
*
* @return the current position
*/
public synchronized double getPosition() {
double pos = m_talon.getPosition();
reportPosition(pos);
return pos;
}