void ControlBoard::UpdateValues() { m_robot->Lock(); //reset these lines so they don't print XXX disabled when enabled m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line4, ""); m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line5, ""); //auton selection if(m_robot->isDisabled) { m_robot->Lock(); if(getBoardButton(kSingleSelect)) { m_robot->currAutonomous=RobotState::kOneTube; m_robot->autonomousDescription = "Auto: One Tube"; } else if (getBoardButton(kDoubleLeftSelect)) { m_robot->currAutonomous=RobotState::kTwoTubeLeft; m_robot->autonomousDescription = "Auto: Two Tube Left"; } else if(getBoardButton(kDoubleRightSelect)) { m_robot->currAutonomous=RobotState::kTwoTubeRight; m_robot->autonomousDescription = "Auto: Two Tube Right"; } else if(getBoardButton(kLowButton)) { m_robot->currAutonomous=RobotState::kNoTube; m_robot->autonomousDescription = "Auto: None"; } else { if(m_robot->currAutonomous == RobotState::kNoTube) { m_robot->autonomousDescription = "Auto: None"; } } m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line1, m_robot->autonomousDescription); //reset the minibot toggle switches and display the status on the LCD //NOTE: these will always be disabled, but it was included just to confirm //their status ResetToggleSwitches(); if(ReleaseMinibot()) m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Releasing Enabled"); else m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Releasing Disabled"); if(DeployMinibot()) m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Deploying Enabled"); else m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Deploying Disabled"); //m_robot->m_lcd->UpdateLCD(); m_robot->Unlock(); } //printf("Got Lock cb\n"); //tests //printf("time: %f\n",m_robot->GetTime()); /* if(getBoardButton(kOperatorScore)) { m_robot->ResetGyro(); m_robot->ResetLeftEncoder(); m_robot->ResetRightEncoder(); } */ // set all the booleans for other control loops // toggle switches m_robot->minibotRelease=ReleaseMinibot(); m_robot->minibotDeploy=DeployMinibot(); m_robot->controlLoopsOn=ControlLoopsOn(); // drivetrain m_robot->straightDrivePower=GetStraightDrivePower(); m_robot->turnPower=GetTurnPower(); //printf("Turn power is %f\n", m_robot->turnPower); m_robot->isQuickTurn=WantQuickTurn(); m_robot->isHighGear=WantHighGear(); //printf("High Gear: %d\n", m_robot->isHighGear); // elevator m_robot->elevatorPower=GetElevatorPower(); SetElevatorHeight(); // arm stow&ground presets // if we're disabled make it just maintain its current position if (getBoardButton(kStow)) { CancelSemiAutoActions(); if(m_robot->isDisabled) { m_robot->armGoal = m_robot->GetArmAngle(); } else { m_robot->armGoal = DegreesToRadians(m_csvReader->GetValue("ARM_UP_ANGLE")); m_robot->elevatorGoal = 0.0; } } else if (getBoardButton(kGroundPickup)) { CancelSemiAutoActions(); if(m_robot->isDisabled) { m_robot->armGoal = m_robot->GetArmAngle(); } else { m_robot->isGrounding = true; m_robot->elevatorGoal = 0.0; } } // rollers m_robot->rollerSpinVal = GetRollerSpinVal(); // grounding if (m_robot->isGrounding) { m_robot->elevatorEnabled=true; if (GetArmPower() != 0 || hasElevatorUserInput()) { CancelSemiAutoActions(); } else if (m_robot->elevatorStopped()) { CancelSemiAutoActions(); m_robot->armGoal = DegreesToRadians(m_csvReader->GetValue("ARM_DOWN_ANGLE")); } } else if (m_robot->isSetpointing) { // setpointing if (GetArmPower() != 0) { CancelSemiAutoActions(); } else { // we need to put the arm upright first //m_robot->armGoal = DegreesToRadians(m_csvReader->GetValue("ARM_UP_ANGLE")); if(!m_arm_is_set_up) { printf("setting up arm\n"); // first store where the elevator wants to go, then wait for the arm to be finished m_robot->elevatorEnabled=false; m_robot->armGoal = DegreesToRadians(m_csvReader->GetValue("ARM_UP_ANGLE")); // the arm is now set up, but the elevator is not m_arm_is_set_up=true; m_elevator_set_up=false; } // once the arm is up and the elevator hasn't been set up, set up the elevator to its desired height else if(m_robot->armStopped() && !m_elevator_set_up) { printf("setting up elevator\n"); m_robot->elevatorEnabled=true; m_elevator_set_up=true; } // once the elevator is set up and done going to its height, set the arm to the right angle if(m_elevator_set_up && m_robot->elevatorStopped()) { printf("setting up arm goal\n"); m_robot->armGoal = DegreesToRadians(m_csvReader->GetValue("ARM_SCORE_ANGLE")); // everything's done - reset the static bools if(m_robot->armStopped()) { CancelSemiAutoActions(); } } } } else //safety - make sure the elevator is enabled m_robot->elevatorEnabled=true; //autoscore if needed if(AutoScoreOn()){ AutoScore(); } m_robot->armPower=GetArmPower(); //give elevator and arm control a little bit of drift double move_angle = DegreesToRadians(m_csvReader->GetValue("ARM_MOVE_SPEED")); double max_up = DegreesToRadians(m_csvReader->GetValue("ARM_UP_ANGLE")); double max_down = DegreesToRadians(m_csvReader->GetValue("ARM_DOWN_ANGLE")); if (m_robot->armPower > 0.1) { m_robot->armGoal += move_angle; } else if (m_robot->armPower < -0.1) { m_robot->armGoal -= move_angle; } else if(fabs(m_prevArmPower)>.1){ m_robot->armGoal = m_robot->GetArmAngle() + .08 * m_robot->armVelocity; } //cap the max arm goal if (m_robot->armGoal <= max_down) m_robot->armGoal = max_down; if (m_robot->armGoal >= max_up) m_robot->armGoal = max_up; m_prevArmPower = m_robot->armPower; //convert buttons of the control loops into desired functions if (m_robot->controlLoopsOn) { if (m_robot->isRollingIn) { if (WantRollOut()) { m_robot->isRollingIn=false; m_robot->isRollingOut=true; } } else { if (WantRollIn()) { m_robot->isRollingIn=true; m_robot->isRollingOut=false; } else { m_robot->isRollingOut=WantRollOut(); } } } else { m_robot->isRollingOut=WantRollOut(); m_robot->isRollingIn=WantRollIn(); } //printf("Release Lock cb\n"); m_robot->Unlock(); }
task main() { bool valid = false; initializeRobot(); waitForStart(); // Get the lift going StartTask(Lift); waitLiftReady(); // Goal to side if(readSonarMax(5) > 130) { //Drive out to not hit wall driveToEncoder(AUTO_DRIVE_SPEED, 150); // Turn left, drive past the beacon, turn back driveToGyro(30, TURN_LEFT); driveToEncoder(AUTO_DRIVE_SPEED, 6250); driveToGyro(120, !TURN_LEFT); // Turn to IR alignment valid = driveToIR(-AUTO_DRIVE_SPEED, true, false, IR_MID); // Goal intermediate or ahead } else { // Drive ahead to get a second reading driveToEncoder(AUTO_DRIVE_SPEED, 2300); // Goal intermediate int sonar = readSonarMax(10); if (sonar == 0 || (sonar <= 90 && sonar >= 70)) { driveToGyro(80, TURN_LEFT); driveToEncoder(AUTO_DRIVE_SPEED, 2500); driveToGyro(115, !TURN_LEFT); // Turn to IR alignment valid = driveToIR(-AUTO_DRIVE_SPEED, true, false, IR_MID); // Goal ahead } else { valid = true; } } // Score if we are aligned if (valid) { valid = AutoScore(); } // Kick if we scored if (valid) { AutoKickstand(); } // Wiggle and flash if something went wrong if (!valid) { servoHookCapture(); wait1Msec(500); servoHookRelease(); } // Always return the lift to COLLECT, even if we failed setWaitLiftCmd(COLLECT); }