// Test Autonomous
void TestAutonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
winch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(0))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0);
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0);
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // Ken
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
winch.Set(0.0);
}
/**
* unchanged from SimpleDemo:
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0.
*/
void OperatorControl(void)
{
DPRINTF(LOG_DEBUG, "OperatorControl");
GetWatchdog().Feed();
while (1)
{
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
}
} // end operator control
/**
* unchanged from SimpleDemo:
*
* Runs the motors under driver control with either tank or arcade
* steering selected by a jumper in DS Digin 0.
*
* added for vision:
*
* Adjusts the servo gimbal based on the color tracked. Driving the
* robot or operating an arm based on color input from gimbal-mounted
* camera is currently left as an exercise for the teams.
*/
void OperatorControl(void)
{
char funcName[] = "OperatorControl";
DPRINTF(LOG_DEBUG, "OperatorControl");
//GetWatchdog().Feed();
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsOperatorControl())
{
setServoPositions(rightStick->GetX(), rightStick->GetY());
/* calculate gimbal position based on color found */
if (FindColor
(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{
foundColor = false;
}
PrintReport(&cReport);
if (!staleImage)
{
if (foundColor)
{
/* Move the servo a bit each loop toward the
* destination. Alternative ways to task servos are
* to move immediately vs. incrementally toward the
* final destination. Incremental method reduces the
* need for calibration of the servo movement while
* moving toward the target.
*/
ShowActivity
("** %s found: Servo: x: %f y: %f",
td.name, horizontalDestination, verticalDestination);
}
else
{
ShowActivity("** %s not found", td.name);
}
}
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
}
while (IsOperatorControl())
{
// determine if tank or arcade mode; default with no jumper is
// for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0)
{
// drive with tank style
myRobot->TankDrive(leftStick, rightStick);
}
else
{
// drive with arcade style (use right stick)
myRobot->ArcadeDrive(rightStick);
}
}
} // end operator control
void Arm::DriveArm() {
float claw_target= CLAW_OPEN;
DriverStation *ds = DriverStation::GetInstance();
if(ds->GetDigitalIn(3) && !ds->GetDigitalIn(4))
{
float tower_target = (ds->GetEnhancedIO().GetAnalogInRatio(1) * (TOWER_MAX - TOWER_MIN)) + TOWER_MIN;
float wrist_target = (ds->GetEnhancedIO().GetAnalogInRatio(3) * (WRIST_MAX-WRIST_MIN)) + WRIST_MIN;
if (tower_target > TOWER_MAX)
tower_target = TOWER_MAX;
else if (tower_target < TOWER_MIN)
tower_target = TOWER_MIN;
if (wrist_target < WRIST_MIN)
wrist_target = WRIST_MIN;
if (wrist_target > WRIST_MAX)
wrist_target = WRIST_MAX;
if (ds->GetDigitalIn(1)) {
claw_target = CLAW_CLOSED;
}
float tower_voltage = towerPID->GetOutput(GetTowerPot(), tower_target, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), claw_target, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), wrist_target, 0,
.2) * -1;
if(!towerLimit->Get() && tower_voltage < 0)
{
tower_voltage = 0;
towerMotor->Set(0);
}
if(fabs(claw_voltage) > .5)
wrist_voltage = 0;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
else if(ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
{
float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_DOWN, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_OPEN, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_DOWN, 0,
.2) * -1;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
else if(!ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
{
float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_UP, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_CLOSED, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_UP, 0,
.2) * -1;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
}