Skip to content

Commit

Permalink
ran formatter (#156)
Browse files Browse the repository at this point in the history
  • Loading branch information
@spacey-sooty
spacey-sooty authored Mar 2, 2024
1 parent 06befea commit 6dc7b68
Show file tree
Hide file tree
Showing 32 changed files with 426 additions and 881 deletions.
108 changes: 52 additions & 56 deletions src/main/cpp/AlphaArm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,89 +4,88 @@

#include "AlphaArm.h"

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)} {}

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(){
void AlphaArm::OnStart() {
_pidArmStates.Reset();
_pidIntakeState.Reset();

}

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

_setAlphaArmVoltage = 0_V;
break;
case AlphaArmState::kRaw:
std::cout << "RawControl" << std::endl;
_setAlphaArmVoltage = _rawArmVoltage;
std::cout << "RawControl" << std::endl;
_setAlphaArmVoltage = _rawArmVoltage;

break;

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

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

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

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() * (2 * 3.1415)))};
break;
std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.48);
_setAlphaArmVoltage = units::volt_t{
_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
break;

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

break;
break;

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

// break;
// break;

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

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

_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());
_config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
_config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))
<< std::endl;
_table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
_table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
_table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));

_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;

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

void AlphaArm::SetState(AlphaArmState state) {
Expand All @@ -97,13 +96,10 @@ void AlphaArm::SetArmRaw(units::volt_t voltage) {
_rawArmVoltage = voltage;
}


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

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


15 changes: 4 additions & 11 deletions src/main/cpp/AlphaArmBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,8 @@ AlphaArmConfig AlphaArm::GetConfig() {
}

void AlphaArmManualControl::OnTick(units::second_t dt) {

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


if (_codriver->GetBButton()) {
if (_rawControl == true) {
Expand All @@ -37,21 +35,16 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
_alphaArm->SetArmRaw(0_V);
}
} else {
if(_codriver->GetLeftTriggerAxis() > 0.1){
if (_codriver->GetLeftTriggerAxis() > 0.1) {
_alphaArm->SetState(AlphaArmState::kSpeakerAngle);
} else if (_codriver->GetLeftBumper()){
} else if (_codriver->GetLeftBumper()) {
_alphaArm->SetState(AlphaArmState::kAmpAngle);
} else if(_codriver->GetYButton()){
} else if (_codriver->GetYButton()) {
_alphaArm->SetState(AlphaArmState::kStowed);
} else if(_codriver->GetRightBumper()){
} else if (_codriver->GetRightBumper()) {
_alphaArm->SetState(AlphaArmState::kIntakeAngle);
} else {
_alphaArm->SetState(AlphaArmState::kIdle);
}
}

}




37 changes: 17 additions & 20 deletions src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
// of the MIT License at the root of this project

#include "Robot.h"
#include "RobotMap.h"

#include <frc/TimedRobot.h>
#include <frc/Timer.h>
#include <frc/controller/RamseteController.h>
Expand All @@ -12,21 +12,18 @@
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <units/acceleration.h>
// #include <units/angle.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <units/voltage.h>


#include "RobotMap.h"
#include "behaviour/HasBehaviour.h"
#include "networktables/NetworkTableInstance.h"


static units::second_t lastPeriodic;

void Robot::RobotInit() {

// shooter = new Shooter(robotmap.shooterSystem.config);
// wom::BehaviourScheduler::GetInstance()->Register(shooter);
// shooter->SetDefaultBehaviour(
Expand All @@ -53,7 +50,6 @@ void Robot::RobotInit() {
// simulation_timer = frc::Timer();

// robotmap.swerveBase.gyro->Reset();


_swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
Expand Down Expand Up @@ -103,8 +99,7 @@ void Robot::RobotInit() {

// _vision = new Vision("limelight", FMAP("fmap.fmap"));

//robotmap->vision = new Vision("limelight", FMAP("fmap.fmap"));

// robotmap->vision = new Vision("limelight", FMAP("fmap.fmap"));
}

void Robot::RobotPeriodic() {
Expand All @@ -113,8 +108,8 @@ void Robot::RobotPeriodic() {

loop.Poll();
wom::BehaviourScheduler::GetInstance()->Tick();
//shooter->OnUpdate(dt);
//sched->Tick();
// shooter->OnUpdate(dt);
// sched->Tick();

// robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
// .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
Expand All @@ -126,16 +121,19 @@ void Robot::RobotPeriodic() {
// .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());

// _swerveDrive->OnUpdate(dt);
//shooter->OnStart();
//intake->OnUpdate(dt);
// shooter->OnStart();
// intake->OnUpdate(dt);

// _swerveDrive->OnUpdate(dt);


robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
.SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
.SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ")
.SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
.SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());

alphaArm->OnUpdate(dt);
_swerveDrive->OnUpdate(dt);
Expand All @@ -159,13 +157,12 @@ void Robot::TeleopInit() {
// backLeft->SetVoltage(4_V);
// backRight->SetVoltage(4_V);


// FMAP("fmap.fmap");

// _swerveDrive->OnStart();
// sched->InterruptAll();

//reimplement when vision is reimplemented
// reimplement when vision is reimplemented

// _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
}
Expand Down Expand Up @@ -196,7 +193,7 @@ void Robot::SimulationInit() {
std::cout << x << std::endl;
std::cout << y << std::endl; */
// std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
//Reimplement when vision is reimplemented
// Reimplement when vision is reimplemented
// frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
// nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
// pose.Rotation().Degrees().value());
Expand Down
12 changes: 0 additions & 12 deletions src/main/cpp/Vision.cpp
View file

This file was deleted.

23 changes: 11 additions & 12 deletions src/main/include/AlphaArm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,15 @@

#pragma once
#include <frc/DigitalInput.h>
#include <frc/controller/PIDController.h>
#include <units/angle.h>
#include <units/voltage.h>

#include <memory>
#include <string>

#include "Wombat.h"
//#include "vision/Vision.h"
#include "utils/PID.h"
#include <units/angle.h>
#include <units/voltage.h>
#include <frc/controller/PIDController.h>

struct AlphaArmConfig {
wom::Gearbox alphaArmGearbox;
Expand All @@ -20,7 +21,6 @@ struct AlphaArmConfig {

std::string path;
// Vision *vision;

};

enum class AlphaArmState {
Expand All @@ -36,25 +36,25 @@ enum class AlphaArmState {

class AlphaArm : public behaviour::HasBehaviour {
public:
AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
explicit AlphaArm(AlphaArmConfig* config);

void OnUpdate(units::second_t dt);
void SetArmRaw(units::volt_t voltage);
void SetState(AlphaArmState state);
void SetControllerRaw(units::volt_t voltage);
void SetControllerRaw(units::volt_t voltage);
void SetGoal(double goal);
void OnStart();
AlphaArmConfig GetConfig(); //{ return _config; }
AlphaArmConfig GetConfig(); //{ return _config; }
frc::PIDController GetPID();

private:
// units::radian_t CalcTargetAngle();

AlphaArmConfig *_config;
AlphaArmConfig* _config;
wom::vision::Limelight* _vision;
AlphaArmState _state = AlphaArmState::kIdle;
//wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
//frc::DutyCycleEncoder armEncoder{4};
// wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
// frc::DutyCycleEncoder armEncoder{4};
frc::PIDController _pidArm;
frc::PIDController _pidArmStates;
frc::PIDController _pidIntakeState;
Expand All @@ -65,5 +65,4 @@ class AlphaArm : public behaviour::HasBehaviour {
units::volt_t _rawArmVoltage = 0_V;
units::volt_t _testRawVoltage = 3_V;
double _goal = 0;

};
Loading

0 comments on commit 6dc7b68

Please sign in to comment.