private bool Unburying(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { logitechController.AXES.LEFT_Y = -1; logitechController.AXES.RIGHT_Y = -1; return(stopwatch.DurationMs - unburyingTime > UNBURYING_TIMOUT); }
private bool LocateVisionTargets(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { float speed = 1.0f; if (NUC_SerialConnection.HasVisionConnection()) { if (!potentialMatch) { potentialMatch = true; potentialMatchTime = stopwatch.DurationMs; } else if ((stopwatch.DurationMs - potentialMatchTime) > POTENTIAL_MATCH_TIMEOUT) { return(true); } if (NUC_SerialConnection.GetAbsolutePosition().yaw > 0) { speed = 0; //Change to less than (yaw < 0) if LEFT_Y set to negative speed } } else { potentialMatch = false; } logitechController.AXES.LEFT_Y = speed; logitechController.AXES.RIGHT_Y = -speed; return(false); }
//Autonomous private void AutonomousMode(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { switch (autonState) { case AutonomousState.STOPPED: logitechController.ResetValues(); break; case AutonomousState.STARTUP: break; case AutonomousState.ORIENT: break; case AutonomousState.HOPPER_LINEUP: HopperLineup.HopperLineupLoop(ref logitechController, ref NUC_SerialConnection); break; default: break; } HopperLineup.HopperLineupLoop(ref logitechController, ref NUC_SerialConnection); }
public static float TurnLoop(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { const float BEGIN_RAMP_DOWN_AT = 15; //DEGREES float yaw = NUC_SerialConnection.GetVisionOrientation_HopperLineup().yaw; float driveSpeed = Controls.ControlAlgorithms.P_Loop(yaw, BEGIN_RAMP_DOWN_AT, 0); logitechController.AXES.LEFT_Y = -driveSpeed; logitechController.AXES.RIGHT_Y = driveSpeed; return(driveSpeed); }
public Robot() { //Initialize a serial connection with the Intel NUC NUC_SerialConnection = new SerialCommsHandler(SerialCommsHandler.Constants.Port.Port1); //Initialize the handler for switching between different control modes (ie Teleop, Autonomous, Test, etc) controlModeHandler = new ControlModeHandler(); //Initialize a logitech joystick object logitechController = new Controller(); //Initialize the drive base motor controllers driveBase = new DriveBase(); }
public bool Update(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { switch (orientState) { case OrientState.INIT: orientState = OrientState.LOCATING_VISION_TARGETS; turningTime = stopwatch.DurationMs; break; case OrientState.LOCATING_VISION_TARGETS: if (LocateVisionTargets(ref logitechController, ref NUC_SerialConnection)) { orientState = OrientState.ORIENTING_TOWARDS_MINING_ZONE; } else if (stopwatch.DurationMs - turningTime > TURNING_TIMOUT) { orientState = OrientState.UNBURYING; unburyingTime = stopwatch.DurationMs; } break; case OrientState.UNBURYING: if (Unburying(ref logitechController, ref NUC_SerialConnection)) { turningTime = stopwatch.DurationMs; orientState = OrientState.LOCATING_VISION_TARGETS; } break; case OrientState.ORIENTING_TOWARDS_MINING_ZONE: logitechController.ResetValues(); Debug.Print("DONE"); break; default: break; } return(false); }
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); }
public static void HopperLineupLoop(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { const float DISTANCE_SCALAR = 1; const float YAW_SCALAR = 2f; const float X_SCALAR = 0.6f; float driveSpeed = DISTANCE_SCALAR * DistanceLoop(ref logitechController, ref NUC_SerialConnection); driveSpeed = TypeConverter.Constrain(driveSpeed, -1.0f, 1.0f); float turnVal = YAW_SCALAR * TurnLoop(ref logitechController, ref NUC_SerialConnection) * (1 - driveSpeed); float horizontalVal = X_SCALAR * HorizontalDisplacementLoop(ref logitechController, ref NUC_SerialConnection); logitechController.AXES.LEFT_Y = -TypeConverter.Constrain(driveSpeed - turnVal + horizontalVal, -0.15f, 1); logitechController.AXES.RIGHT_Y = -TypeConverter.Constrain(driveSpeed + turnVal - horizontalVal, -0.15f, 1); //Debug.Print(logitechController.AXES.LEFT_Y.ToString() + ", " + logitechController.AXES.RIGHT_Y.ToString()); }
//Applies different values to the controller object based on the current control mode public void updateControllerValues(ref Controller controller, ref SerialCommsHandler serial) { isRobotActive = false; switch (mode) { //Use sensors and algorithms to apply values to the controller instead of a human operator case ControlMode.AUTONOMOUS: AutonomousMode(ref controller, ref serial); isRobotActive = true; break; //Read in the human-controlled joystick over the serial connection case ControlMode.TELEOP: TeleopMode(ref controller, ref serial); //Activate the robot in teleop mode if the dead man's switch is held isRobotActive = controller.BUTTONS.LB; break; //For testing new code/functions case ControlMode.TEST: TestMode(ref controller, ref serial); isRobotActive = true; break; //Do nothing case ControlMode.DISABLED: controller.ResetValues(); break; //Invalid mode - print an error message default: Debug.Print("ERROR - Invalid Mode: " + mode.ToString()); break; } }
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); }
//For running test code, testing new functions private void TestMode(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { //HopperLineup.HorizontalDisplacementLoop(ref logitechController, ref NUC_SerialConnection); orient.Update(ref logitechController, ref NUC_SerialConnection); }
//Update logitechController values with those sent wirelessly from the driver station private void TeleopMode(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection) { NUC_SerialConnection.UpdateJoystickValues(ref logitechController); }