Last minute, our team decided to add two servos to control one pole. We did a quick makeshift code but it doesn’t work. We just added the second servo (which is positioned on the other side of the pole) and added 180 to the angle.
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.command.Subsystem;
import frc.robot.commands.ServoCommand;
//Imports data from other classes, variant is REQUIRED for subsystems
import static frc.robot.RobotMap.*;
import static frc.robot.OI.*;
//Allows use of the existing joystick
import edu.wpi.first.wpilibj.Servo;
//Allows use of servo methods
public class ServoSubsystem extends Subsystem {
public Servo servo_0;
public Servo servo_1;
public int defaultAngle;
public ServoSubsystem() { //Constructor, Finn's huge mistake
servo_0 = new Servo(SRV_PRT_0); //Sets port for servo
servo_1 = new Servo(SRV_PRT_1); //Sets port for servo
defaultAngle = 0; //Default angle for use later, change to set default
}
public void initDefaultCommand() { //Runs on startup
servo_0.setAngle(defaultAngle); //Moves servo to default position
servo_1.setAngle(180 + defaultAngle); //Moves servo to default position
setDefaultCommand(new ServoCommand()); //Variant is REQUIRED for subsystems
}
public void rotateArm() {
if(leftJoystick.getRawButton(SRV_Pos)){
if(servo_0.getAngle() < defaultAngle + 90) {
servo_0.setAngle(servo_0.getAngle() + SRV_Spd);
servo_1.setAngle(servo_1.getAngle() - SRV_Spd);
}
}
if(leftJoystick.getRawButton(SRV_Neg)){
if(servo_0.getAngle() > defaultAngle + 3) {
servo_0.setAngle(servo_0.getAngle() - SRV_Spd);
servo_1.setAngle(servo_1.getAngle() + SRV_Spd);
}
}
}
}
The issue is that the second servo does not seem to be moving at all.