void DriveSubsystem::AutoModeDisable() {
if (autoMode != Off) {
if (autoMode == Driving)
{
// Cancel Driving
if (autoActionState == InProgress)
{
SetMotorSpeeds(0.0);
}
}
else if (autoMode == Rotating) {
if (autoActionState == InProgress)
{
robotDrive->SetLeftRightMotorOutputs(0, 0);
}
}
autoMode = Off;
autoActionState = NotStarted;
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kINFO,"Auto Drive Mode DISABLED\n");
}
else {
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kINFO,"Auto Drive Mode ALREADY disabled,IGNNORING\n");
}
}
void RobotDriveControl::Disable()
{
m_left_speed = 0.0f;
m_right_speed = 0.0f;
m_current_left_speed = 0.0f;
m_current_right_speed = 0.0f;
m_shift_factor = m_initial_shift;
SetMotorSpeeds();
}
void RobotDriveControl::Update(double delta)
{
// Handle the shift input
bool shift_changed = false;
if(m_joystick->IsPressed(XBOX_360_BUTTON_R_BUMPER))
{
m_shift_factor += m_shift_step;
shift_changed = true;
}
if(m_joystick->IsPressed(XBOX_360_BUTTON_L_BUMPER))
{
m_shift_factor -= m_shift_step;
shift_changed = true;
}
if (shift_changed)
{
m_shift_factor = MotorControlHelper::Limit(m_shift_factor, m_min_shift, m_max_shift);
SmartDashboard::PutNumber("Drive/Shift/Current", m_shift_factor);
}
// Handle the stick inputs
m_left_speed = m_joystick->GetAxis(XBOX_360_AXIS_Y_LEFT);
m_right_speed = m_joystick->GetAxis(XBOX_360_AXIS_Y_RIGHT);
int pov = m_joystick->GetPOV();
SmartDashboard::PutNumber("Drive/POV", pov);
if (pov >= XBOX_360_POV_DIR_UP)
{
switch (pov)
{
case XBOX_360_POV_DIR_UP:
m_left_speed = -1.0f;
m_right_speed = -1.0f;
break;
case XBOX_360_POV_DIR_UP_RIGHT:
m_left_speed = -1.0f;
m_right_speed = 0.0f;
break;
case XBOX_360_POV_DIR_RIGHT:
m_left_speed = -1.0f;
m_right_speed = 1.0f;
break;
case XBOX_360_POV_DIR_DOWN_RIGHT:
m_left_speed = 0.0f;
m_right_speed = 1.0f;
break;
case XBOX_360_POV_DIR_DOWN:
m_left_speed = 1.0f;
m_right_speed = 1.0f;
break;
case XBOX_360_POV_DIR_DOWN_LEFT:
m_left_speed = 1.0f;
m_right_speed = 0.0f;
break;
case XBOX_360_POV_DIR_LEFT:
m_left_speed = 1.0f;
m_right_speed = -1.0f;
break;
case XBOX_360_POV_DIR_UP_LEFT:
m_left_speed = 0.0f;
m_right_speed = -1.0f;
break;
}
}
m_left_speed = MotorControlHelper::Limit(m_left_speed, -1.0f, 1.0f) * m_shift_factor;
m_right_speed = MotorControlHelper::Limit(m_right_speed, -1.0f, 1.0f) * m_shift_factor;
// Set the output
SetMotorSpeeds();
}
void SetMotorSpeedsNoReturn(signed int left, signed int right, signed long winkel)
{
SetMotorSpeeds(&left, &right, winkel);
}
void DriveSubsystem::AutoDriveMakeProgress(double distanceTraveledSoFar[]) {
//RotateDirection being a new enum with just left and right
// or we can find the shortest distance by comparing the
if (autoMode == Driving)
{
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO, "AutoDriveMakeProgress:: Step 1");
if (autoActionState == InProgress)
{
double distanceFromGoal[4];
AutoDriveCalcDistanceFromGoal(distanceTraveledSoFar, distanceFromGoal);
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO, "AutoDriveMakeProgress:: Step 2");
// If the distance from the goal is NOT within the epsilon AND
// if the distance from the targeted distance has not grown
// since the last distance calculation apply voltage.
if (!AutoDriveShouldStop(distanceFromGoal)) {
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO, "AutoDriveMakeProgress:: Step 3");
// Calculate the speed that the Robot should start rotating at.
float slowDownPercentageOfSpeed = 1.0; /*CalcPercentageOfMaxSpeed(distanceFromGoal[0],
autoDrivingParams.totalRotationsToDesiredPosition,
autoDrivingParams.slowDownRotationCountThreshold); */
if ((autoDrivingParams.autoHeading == DriveForward) || (autoDrivingParams.autoHeading == DriveBack))
{
AutoDriveFrontLeftMotorSpeed *= slowDownPercentageOfSpeed * AutoDriveForwardBackHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveBackLeftMotorSpeed *= slowDownPercentageOfSpeed * AutoDriveForwardBackHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveFrontRightMotorSpeed *= slowDownPercentageOfSpeed * AutoDriveForwardBackHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveBackRightMotorSpeed *= slowDownPercentageOfSpeed * AutoDriveForwardBackHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
}
else
{
AutoDriveFrontLeftMotorSpeed *= AutoDriveLeftRightHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveBackLeftMotorSpeed *= AutoDriveLeftRightHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveFrontRightMotorSpeed *= AutoDriveLeftRightHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
AutoDriveBackRightMotorSpeed *= AutoDriveLeftRightHeadingSpeedScaleFactor * autoDrivingParams.speedScaleFactor;
}
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: InProgress, distanceFromGoal=%g, distanceSoFar=%g, absFrontLeftSpeed=%f, absBackLeftSpeed=%f, absFrontRightSpeed=%f, absBackRightSpeed=%f\n",
distanceFromGoal[0],
distanceTraveledSoFar[0],
AutoDriveFrontLeftMotorSpeed,
AutoDriveBackLeftMotorSpeed,
AutoDriveFrontRightMotorSpeed,
AutoDriveBackRightMotorSpeed);
switch (autoDrivingParams.autoHeading)
{
case DriveForward:
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: Heading Forward\n");
SetMotorSpeeds(-AutoDriveFrontLeftMotorSpeed,
-AutoDriveBackLeftMotorSpeed,
AutoDriveFrontRightMotorSpeed,
AutoDriveBackRightMotorSpeed);
break;
case DriveBack:
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: Heading Backward\n");
SetMotorSpeeds(AutoDriveFrontLeftMotorSpeed,
AutoDriveBackLeftMotorSpeed,
-AutoDriveFrontRightMotorSpeed,
-AutoDriveBackRightMotorSpeed);
break;
case DriveLeft:
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: Heading Left\n");
SetMotorSpeeds(AutoDriveFrontLeftMotorSpeed,
-AutoDriveBackLeftMotorSpeed,
AutoDriveFrontRightMotorSpeed,
-AutoDriveBackRightMotorSpeed);
break;
case DriveRight:
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: Heading Right\n");
SetMotorSpeeds(-AutoDriveFrontLeftMotorSpeed,
AutoDriveBackLeftMotorSpeed,
-AutoDriveFrontRightMotorSpeed,
AutoDriveBackRightMotorSpeed);
break;
}
}
else {
// Hit the stopping point, shutdown.
SetMotorSpeeds(0.0);
autoActionState = Completed;
Logger::GetInstance()->Log(DriveSubsystemLogId, Logger::kINFO,"AutoDriveMakeProgress: InProgress, Stopping Motors\n");
}
}
}
else {
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kERROR,"***Auto Rotation not setup or already Rotating (%s), IGNNORING***\n", AutoModeToString(autoMode).c_str());
}
}
void DriveSubsystem::AutoDriveExecute()
{
if (autoMode == Driving) {
if (autoActionState == Completed)
{
return;
}
double distanceTraveledSoFar[4];
if (autoActionState == NotStarted) {
// Indicate that the rotation is now in progress.
autoActionState = InProgress;
// here is where we would start the PID or the manual encoder code
SetMotorSpeeds(0.0);
cANTalon1->SetPosition(0.0);
cANTalon2->SetPosition(0.0);
cANTalon3->SetPosition(0.0);
cANTalon4->SetPosition(0.0);
Wait(0.100);
AutoDriveReadMotorRotationCounts();
autoDrivingParams.positionCountAtStartOfAutoDrive[0] = autoDrivingParams.lastPositionReading[0];
autoDrivingParams.lastDistanceFromGoal[0] = autoDrivingParams.totalRotationsToDesiredPosition - (autoDrivingParams.lastPositionReading[0] - autoDrivingParams.positionCountAtStartOfAutoDrive[0]);
if (!AutoDriveUseOneMotorForDistanceDrive)
{
autoDrivingParams.positionCountAtStartOfAutoDrive[1] = autoDrivingParams.lastPositionReading[1];
autoDrivingParams.positionCountAtStartOfAutoDrive[2] = autoDrivingParams.lastPositionReading[2];
autoDrivingParams.positionCountAtStartOfAutoDrive[3] = autoDrivingParams.lastPositionReading[3];
autoDrivingParams.lastDistanceFromGoal[1] = autoDrivingParams.totalRotationsToDesiredPosition - (autoDrivingParams.lastPositionReading[1] - autoDrivingParams.positionCountAtStartOfAutoDrive[1]);
autoDrivingParams.lastDistanceFromGoal[2] = autoDrivingParams.totalRotationsToDesiredPosition - (autoDrivingParams.lastPositionReading[2] - autoDrivingParams.positionCountAtStartOfAutoDrive[2]);
autoDrivingParams.lastDistanceFromGoal[3] = autoDrivingParams.totalRotationsToDesiredPosition - (autoDrivingParams.lastPositionReading[3] - autoDrivingParams.positionCountAtStartOfAutoDrive[3]);
}
distanceTraveledSoFar[0] = 0.0;
distanceTraveledSoFar[1] = 0.0;
distanceTraveledSoFar[2] = 0.0;
distanceTraveledSoFar[3] = 0.0;
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kINFO,"AutoDriveExecute: Switching to InProgress, (position, lastPostion)=%g, lastDistance=%gMode\n",
autoDrivingParams.lastPositionReading[0],
autoDrivingParams.lastDistanceFromGoal[0]);
}
else {
AutoDriveReadMotorRotationCounts();
distanceTraveledSoFar[0] = autoDrivingParams.lastPositionReading[0] - autoDrivingParams.positionCountAtStartOfAutoDrive[0];
distanceTraveledSoFar[0] = ImposeMinimumOnValue(distanceTraveledSoFar[0], 0.0);
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kINFO,"AutoDriveExecute: InProgress, lastPostion=%g, distanceSoFar=%g, prevDistance=%g\n",
autoDrivingParams.lastPositionReading[0],
distanceTraveledSoFar[0],
autoDrivingParams.lastDistanceFromGoal[0]);
if (!AutoDriveUseOneMotorForDistanceDrive)
{
distanceTraveledSoFar[1] = ImposeMinimumOnValue(autoDrivingParams.lastPositionReading[1] - autoDrivingParams.positionCountAtStartOfAutoDrive[1], 0.0);
distanceTraveledSoFar[2] = ImposeMinimumOnValue(autoDrivingParams.lastPositionReading[2] - autoDrivingParams.positionCountAtStartOfAutoDrive[2], 0.0);
distanceTraveledSoFar[3] = ImposeMinimumOnValue(autoDrivingParams.lastPositionReading[3] - autoDrivingParams.positionCountAtStartOfAutoDrive[3], 0.0);
}
}
AutoDriveMakeProgress(distanceTraveledSoFar);
}
else {
Logger::GetInstance()->Log(DriveSubsystemLogId,Logger::kERROR,"***Auto Driving not setup or already Auto Driving (%s), IGNNORING***\n", AutoModeToString(autoMode).c_str());
}
}
