void RedIsolationPickupAutonomous()
{
SensorValue[Gyroscope] = 0;
StartTask(KeepArmInPosition);
armPosition = minArm;
Intake(127);
Forward(45, 125, "none");
StopMoving();
Backward(127, 90, "none");
Intake(0);
StopMoving();
TurnLeftDegrees(40);
armPosition = midGoalHeight;
wait10Msec(70);
Intake(127);
Forward(100, 60, "none");
StopMoving();
armPosition = midGoalHeight - 100;
wait10Msec(50);
StopTask(KeepArmInPosition);
StopArm();
StartTask(StayInPosition);
wait10Msec(500);
Intake(0);
wait10Msec(1000);
StopTask(StayInPosition);
}
//------------------------------------------------------------------------------
// METHOD: ArmLower::GoToPotTarget()
// Type: Private method
//------------------------------------------------------------------------------
// Moves the arm Arm until it reaches the target position, or activates
// the upper or lower limit switches.
// *** NOTE TO ALLOW THE LIMIT SWITCHES TO READ THE WAY WE WANT THEM TO IN THE
// ** CODE - WIRE THEM "NORMAL CLOSED" OR NC.
//------------------------------------------------------------------------------
bool ArmLower::GoToPOTTarget(double inputPotValue)
{
bool potTargetFound = false;
double targetLowValue = inputPotValue - TARGET_TOLERANCE;
double targetHighValue = inputPotValue + TARGET_TOLERANCE;
if ( pArmPot->Get() >= targetLowValue &&
pArmPot->Get() <= targetHighValue )
{
StopArm();
potTargetFound = true;
}
else
{
if ( pArmPot->Get() > targetHighValue ) // Arm moving down
{
MoveArmUp();
}
else
{
if ( pArmPot->Get() < targetLowValue ) // Arm moving up
{
MoveArmDown();
}
}
}
return potTargetFound;
}
//------------------------------------------------------------------------------
// METHOD: ArmLower::MoveArmDown()
// Type: Public accessor method
//------------------------------------------------------------------------------
// Calculates a target robot POT value based on input target position
// and then moves the Arm to the desired position.
//------------------------------------------------------------------------------
void ArmLower::MoveArmDown()
{
if ( pArmPot->Get() >= OUTPUT_POT_FULL_BACK )
StopArm();
else
RunArmMotor(MOTOR_SPEED_DOWN);
return;
}
//------------------------------------------------------------------------------
// METHOD: ArmLower::MoveArmUp()
// Type: Public accessor method
//------------------------------------------------------------------------------
// Calculates a target robot POT value based on input target position
// and then moves the Arm to the desired position.
//------------------------------------------------------------------------------
void ArmLower::MoveArmUp()
{
if ( pArmStopSensor->Get() ||
pArmPot->Get() <= OUTPUT_POT_FULL_FWD )
StopArm();
else
RunArmMotor(MOTOR_SPEED_UP);
return;
}
// Optionally raises the arm to clear the bridge, then lowers it.
// TODO - Get minimum angle to clear bridge in neutral position
void TipBridge::Execute() {
if (needsReset) {
if (!moving) { RaiseArm(); }
if (accelerometer->GetArmDegree() > 35.0) {
needsReset = false;
StopArm();
}
} else {
LowerArm();
}
}
//Controls motion of arm
void Arm::OperateArm() {
AutoArm();
//Controlled by right analog stick
if (xboxController->GetRawAxis(5) >= 0.2 || xboxController->GetRawAxis(5) <= -0.2) { //Absolute value of controller stick
RaiseArm(xboxController->GetRawAxis(5));
}
//Controlled by hat switch on top of joystick
else if (joystick->GetRawAxis(6) > 0.0)
RaiseArm(1);
else if (joystick->GetRawAxis(6) < -0.0)
LowerArm(1);
else if (!automatic)
StopArm();
}
void IsolationBlockAutonomous()
{
SensorValue[Gyroscope] = 0;
StartTask(KeepArmInPosition);
armPosition = midGoalHeight;
wait10Msec(25);
Intake(127);
Forward(100, 80, "none");
StopMoving();
armPosition = midGoalHeight - 100;
wait10Msec(80);
StopTask(KeepArmInPosition);
StopArm();
StartTask(StayInPosition);
wait10Msec(500);
Intake(0);
wait10Msec(1000);
StopTask(StayInPosition);
}
void IsolationScoringAutonomous()
{
SensorValue[Gyroscope] = 0;
StartTask(KeepArmInPosition);
armPosition = minArm;
Intake(127);
Forward(45, 135, "none");
Backward(127, 35, "none");
Intake(0);
armPosition = midGoalHeight+50;
Backward(127, 80, "none");
StopMoving();
while (SensorValue[Bump] == 0) {}
Forward(65, 60, "none");
StopMoving();
wait10Msec(20);
Intake(-127);
wait10Msec(90);
Intake(70);
Backward(35, 30, "none");
StopMoving();
Intake(-127);
wait10Msec(120);
Intake(0);
//VariableMove(-70, -20, 80);
Backward(45, 20, "none");
armPosition = minArm;
Backward(45, 20, "none");
StopMoving();
wait10Msec(200);
StopTask(KeepArmInPosition);
StopArm();
}
// When interrupted, stop the arm before handing off to
// the interrupting command.
void TipBridge::Interrupted() {
StopArm();
}
// All done - stop the arm
void TipBridge::End() {
StopArm();
}
