Develop a feedback loop to manipulate the wrist motor

[Team1710CTO]

The logic seems to work fine for controlling the motor (controller inputs) however, we now need to set power to the motor based on the error between our current position and goal. Take a look at Nathan's code in the lift class. This: RobotMap.lift1.set(ControlMode.PercentOutput, (-1 * (((setPoint - getHeight()) * Constants.kPLift)))) is pretty similar to how the wrist will need to be controlled. Setpoint is just one of the constants for the wrist's positions, getHeight() is the equivalent of getting the current value from the wrist encoder, and you will need to create a new gain constant (similar to Constants.kPlift).

[mollymaples]

I made the code that mainpulates the wrist position and motor power. public static void mainpulateWrist() { RobotMap.wrist.set(ControlMode.PercentOutput, getWristError() * Constants.kPWrist); }