public static float HorizontalDisplacementLoop(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { float dist = NUC_SerialConnection.GetVisionLocation_HopperLineup().x; const float BEGIN_RAMP_DOWN_AT = 20; float driveSpeed = Controls.ControlAlgorithms.P_Loop(dist, BEGIN_RAMP_DOWN_AT); logitechController.AXES.LEFT_Y = driveSpeed; logitechController.AXES.RIGHT_Y = driveSpeed; Debug.Print(driveSpeed.ToString()); return(driveSpeed); }
public static float DistanceLoop(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { const float BEGIN_RAMP_DOWN_AT = 26; //INCHES const float STOPPING_DISTANCE = 20; //INCHES float dist = NUC_SerialConnection.GetVisionLocation_HopperLineup().z; float driveSpeed = Controls.ControlAlgorithms.P_Loop(dist, BEGIN_RAMP_DOWN_AT, STOPPING_DISTANCE); logitechController.AXES.LEFT_Y = driveSpeed; logitechController.AXES.RIGHT_Y = driveSpeed; //Debug.Print(driveSpeed.ToString()); return(driveSpeed); }