Skip to content

Commit

Permalink
Toby Beta (#151)
Browse files Browse the repository at this point in the history 
* setpoints

* Arm Good PID States

* Cleaned up PID arm

* Finished Arm SetStates
  • Loading branch information
@JoystickMaster-race
JoystickMaster-race authored Feb 29, 2024
1 parent be5e35a commit af62b5d
Show file tree
Hide file tree
Showing 12 changed files with 649 additions and 127 deletions.
117 changes: 80 additions & 37 deletions src/main/cpp/AlphaArm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,63 +4,106 @@

#include "AlphaArm.h"

AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}

AlphaArmConfig AlphaArm::GetConfig() {
return _config;
AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)}
{

}

void AlphaArm::OnStart(){
_pidArmStates.Reset();
_pidIntakeState.Reset();

}

void AlphaArm::OnUpdate(units::second_t dt) {
switch (_state) {
case AlphaArmState::kIdle:

// _config.alphaArmGearbox.motorController->SetVoltage(0_V);
// _config.wristGearbox.motorController->SetVoltage(0_V);

_setAlphaArmVoltage = 0_V;
_setWristVoltage = 0_V;

break;
case AlphaArmState::kRaw:
_setAlphaArmVoltage = _rawArmVoltage;
_setWristVoltage = _rawWristVoltage;
_config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
_config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);

std::cout << "RawControl" << std::endl;
_setAlphaArmVoltage = _rawArmVoltage;

break;
case AlphaArmState::kForwardWrist:
_config.wristGearbox.motorController->SetVoltage(3_V);
_setWristVoltage = 3_V;

case AlphaArmState::kReverseWrist:
_config.wristGearbox.motorController->SetVoltage(-3_V);
_setWristVoltage = -3_V;
default:
std::cout << "oops, wrong state" << std::endl;

case AlphaArmState::kHoldAngle:
{
_pidArm.SetSetpoint(-_goal);
//_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
}
break;
case AlphaArmState::kAmpAngle:
std::cout << "Amp Angle" << std::endl;

_pidArmStates.SetSetpoint(-2.17);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};

break;

case AlphaArmState::kIntakeAngle:
std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.48); //-0.48
_setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
break;

case AlphaArmState::kSpeakerAngle:
std::cout << "Speaker Angle" << std::endl;
_pidArmStates.SetSetpoint(-0.82);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};

break;

// case AlphaArmState::kVisionAngle:
// std::cout << "Vision Angle" << std::endl;

// break;

case AlphaArmState::kStowed:
std::cout << "Stowed" << std::endl;
_pidArmStates.SetSetpoint(-0.52);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
default:
std::cout << "Error: alphaArm in INVALID STATE" << std::endl;
break;

}
// transmission translate
// _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
// _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
// _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
_config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
_config.wristGearbox.motorController->SetVoltage(_setWristVoltage);

// _config.wristGearbox.motorController->SetVoltage(_setVoltage);
_config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
_config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
std::cout << "Encoder Value: " << _config->alphaArmEncoder.GetEncoderPosition().value() << std::endl;
_table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
_table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
_table->GetEntry("Input").SetDouble(_config->alphaArmEncoder.GetEncoderPosition().value());

_table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
_table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());

_table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
_table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());



std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;

}

void AlphaArm::SetState(AlphaArmState state) {
_state = state;
}

// void AlphaArm::SetRaw(units::volt_t voltage){
// _rawArmVoltage = voltage;
// _rawWristVoltage = voltage;
// }

void AlphaArm::SetArmRaw(units::volt_t voltage) {
_rawArmVoltage = voltage;
}

void AlphaArm::setWristRaw(units::volt_t voltage) {
_rawWristVoltage = voltage;

void AlphaArm::SetGoal(double goal){
_goal = goal;
}

void AlphaArm::SetControllerRaw(units::volt_t voltage){
_controlledRawVoltage = voltage;
}


33 changes: 22 additions & 11 deletions src/main/cpp/AlphaArmBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,25 +11,36 @@ AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxContro
Controls(alphaArm);
}

AlphaArmConfig AlphaArm::GetConfig() {
return *_config;
}

void AlphaArmManualControl::OnTick(units::second_t dt) {
if (_codriver->GetXButtonPressed()) {

_table->GetEntry("State").SetBoolean(_rawControl);
_table->GetEntry("Goal Value").SetBoolean(_gotValue);


if (_codriver->GetBButton()) {
if (_rawControl == true) {
_rawControl = false;
} else {
_rawControl = true;
}
}

if (_rawControl) {
_alphaArm->SetState(AlphaArmState::kRaw);
_alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
_alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
if(_codriver->GetLeftTriggerAxis() > 0.1){
_alphaArm->SetState(AlphaArmState::kSpeakerAngle);
} else if (_codriver->GetLeftBumper()){
_alphaArm->SetState(AlphaArmState::kAmpAngle);
} else if(_codriver->GetYButton()){
_alphaArm->SetState(AlphaArmState::kStowed);
} else if(_codriver->GetRightBumper()){
_alphaArm->SetState(AlphaArmState::kIntakeAngle);
} else {
if (_codriver->GetRightBumperPressed()) {
_alphaArm->SetState(AlphaArmState::kForwardWrist);
}
if (_codriver->GetLeftBumperPressed()) {
_alphaArm->SetState(AlphaArmState::kReverseWrist);
}
_alphaArm->SetState(AlphaArmState::kIdle);
}
}



Loading

0 comments on commit af62b5d

Please sign in to comment.