std::string OI::NumberToStringElevatorPower() {
std::ostringstream power; //output string stream
power << "Power = " << GetElevatorPower(); //use the string stream just like cout,except the stream prints not to stdout but to a string.
std::string powerToString = power.str(); //the str() function of the stream returns the string.
return powerToString;
}
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();
}
