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);
}
