Skip to content

Commit f1dc6eb

Browse files
danielbrownmsmokcroboticsteamjkleiber
authored
Feat/arm subsystem (#36)
* pid started * arm pids * fixed problems kinda? * fixed errors * fixed errors * changed nothing actually * #3: Fixes merge mayhem for the arm subsystem * #3: renames files so they actually build. --------- Co-authored-by: okcroboticsteam <okcroboticsteam@gmail.com> Co-authored-by: Justin Kleiber <jkleiber8@gmail.com>
1 parent 4f1f643 commit f1dc6eb

File tree

4 files changed

+254
-0
lines changed

4 files changed

+254
-0
lines changed

‎src/main/cpp/io/ArmIO.cpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
2+
#include "io/ArmIO.h"
3+
4+
void ArmIO::Periodic() {
5+
// Process all the inputs and outputs to/from high level software.
6+
VOKC_CALL(ProcessIO());
7+
}
8+
9+
void ArmIO::SimulationPeriodic() {
10+
// SimulationPeriodic
11+
}
12+
13+
bool ArmIO::ProcessIO() {
14+
OKC_CHECK(sw_interface_ != nullptr);
15+
OKC_CHECK(hw_interface_ != nullptr);
16+
17+
// Set the software outputs
18+
// If the intake configuration needs to be updated, update it.
19+
if (sw_interface_->update_config) {
20+
OKC_CALL(UpdateArmConfig(sw_interface_->arm_config));
21+
22+
// Lower the update flag
23+
sw_interface_->update_config = false;
24+
}
25+
26+
// If the encoder should be reset, reset it
27+
if (sw_interface_->reset_encoders) {
28+
OKC_CALL(ResetEncoders());
29+
30+
// Lower the encoder reset flag
31+
sw_interface_->reset_encoders = false;
32+
}
33+
34+
// if the encoder should be set to a specific value
35+
if (sw_interface_->set_encoder_to_val) {
36+
OKC_CALL(SetEncoder(sw_interface_->encoder_val_to_set));
37+
38+
sw_interface_->set_encoder_to_val = false;
39+
}
40+
41+
// Get the hardware sensor values.
42+
// limit switches
43+
sw_interface_->deployed_limit_switch_val =
44+
hw_interface_->deploy_limit_switch->Get(); //???
45+
sw_interface_->retracted_limit_switch_val =
46+
hw_interface_->retracted_limit_switch->Get(); //???
47+
48+
// intake position encoder
49+
sw_interface_->arm_position_encoder_val =
50+
hw_interface_->arm_position_motor->GetEncoder().GetPosition();
51+
52+
return true;
53+
}
54+
55+
bool ArmIO::UpdateArmConfig(ArmConfig &config) {
56+
OKC_CHECK(hw_interface_ != nullptr);
57+
58+
// Get the configuration
59+
double open_loop_ramp = config.open_loop_ramp_rate;
60+
// double max_output_deploy = config.max_output_deploy;
61+
// double max_output_retract = config.max_output_retract;
62+
double max_indexer_current = config.max_indexer_current;
63+
64+
// Apply the configuration
65+
// Open Loop Ramp Rate
66+
hw_interface_->arm_lift_motor->SetOpenLoopRampRate(open_loop_ramp);
67+
hw_interface_->indexer_motor->SetOpenLoopRampRate(open_loop_ramp);
68+
69+
// Java code has this commented out as well, I'm assuming because it was
70+
// meesing something up. well, the intake works (probably, been a while
71+
// since it's been actually plugged in) without this, so leaving commented
72+
// out for now
73+
// hw_interface_->intake_position_motor->SetOpenLoopRampRate(open_loop_ramp);
74+
75+
// current limiting, so the neo 550 on the indexer doesn't stall and smoke
76+
hw_interface_->indexer_motor->SetSmartCurrentLimit(max_indexer_current);
77+
78+
return true;
79+
}
80+
81+
bool ArmIO::ResetEncoders() {
82+
OKC_CHECK(hw_interface_ != nullptr);
83+
84+
hw_interface_->arm_position_motor->GetEncoder().SetPosition(0.0);
85+
86+
// we currently don't use the encoder for these other two motors, so we
87+
// don't need to reset them yet. yet.
88+
// hw_interface_->intake_motor->GetEncoder().SetPosition(0.0);
89+
// hw_interface_->indexer_motor->GetEncoder().SetPosition(0.0);
90+
91+
return true;
92+
}
93+
94+
bool ArmIO::SetEncoder(double &val) {
95+
OKC_CHECK(hw_interface_ != nullptr);
96+
97+
hw_interface_->arm_position_motor->GetEncoder().SetPosition(val);
98+
99+
return true;
100+
}

‎src/main/cpp/subsystems/Arm.cpp‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
#include "subsystems/Arm.h"
2+
3+
bool Arm::Init() {
4+
arm_pid_ = std::make_shared<frc::PIDController>(0, 0, 0);
5+
6+
inches_pid_ = std::make_shared<frc::PIDController>(0, 0, 0);
7+
return true;
8+
}
9+
10+
bool Arm::SetDegrees(double degrees) {
11+
this->arm_pid_->SetSetpoint(degrees);
12+
13+
return true;
14+
}
15+
16+
bool Arm::SetExtend(double inches) {
17+
this->inches_pid_->SetSetpoint(inches);
18+
19+
return true;
20+
}
21+
22+
void Arm::Periodic() {
23+
this->interface_->arm_lift_power =
24+
this->arm_pid_->Calculate(this->interface_->arm_encoder);
25+
this->interface_->arm_extend_power =
26+
this->inches_pid_->Calculate(this->interface_->arm_extend_encoder);
27+
}
28+
29+
// TODO: Understand this example code and implement it correctly
30+
// Note: this is public in the header file
31+
// bool Subsystem::SetPosition(const double &position) {
32+
// this.position_ = position;
33+
// return true;
34+
// }
35+
36+
// void Subsystem::Periodic() {
37+
// // The subsystem does different things based on what mode it is put in
38+
// // by the user.
39+
// switch (mode) {
40+
// case Manual:
41+
// OKC_CALL(SetUserPower());
42+
// break;
43+
// case AutoPosition:
44+
// OKC_CALL(GoToPosition());
45+
// break;
46+
// default:
47+
// break;
48+
// }
49+
// }
50+
51+
// bool Subsystem::SetPosition(const double &position) {
52+
// this.position_ = position;
53+
// return true;
54+
// }

‎src/main/include/io/ArmIO.h‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
#pragma once
2+
3+
#include <memory>
4+
5+
#include <frc/DigitalInput.h>
6+
#include <frc/motorcontrol/MotorControllerGroup.h>
7+
#include <frc2/command/SubsystemBase.h>
8+
#include <rev/CANSparkMax.h>
9+
10+
#include "Utils.h"
11+
12+
typedef struct arm_config_t {
13+
double open_loop_ramp_rate;
14+
double max_indexer_current;
15+
} ArmConfig;
16+
17+
typedef struct arm_hardware_interface_t {
18+
std::unique_ptr<rev::CANSparkMax> arm_lift_motor;
19+
std::unique_ptr<rev::CANSparkMax> arm_up_motor;
20+
std::unique_ptr<rev::CANSparkMax> arm_extend_motor;
21+
std::unique_ptr<rev::CANSparkMax> arm_position_motor;
22+
std::unique_ptr<rev::CANSparkMax> indexer_motor;
23+
std::unique_ptr<rev::CANSparkMax> retracted_limit_switch;
24+
std::unique_ptr<rev::CANSparkMax> deploy_limit_switch;
25+
} ArmHardwareInterface;
26+
27+
typedef struct arm_software_interface_t {
28+
29+
// actuator outputs
30+
double arm_lift_power;
31+
double arm_up_power;
32+
double arm_extend_power;
33+
double arm_encoder;
34+
double arm_extend_encoder;
35+
double arm_position_motor;
36+
ArmConfig arm_config;
37+
double encoder_val_to_set;
38+
39+
bool update_config;
40+
bool reset_encoders;
41+
bool set_encoder_to_val;
42+
bool deployed_limit_switch_val;
43+
bool deploy_limit_switch;
44+
bool retracted_limit_switch_val;
45+
bool retracted_limit_switch;
46+
double arm_position_encoder_val;
47+
double open_loop_ramp;
48+
double max_indexer_current;
49+
} ArmSoftwareInterface;
50+
51+
class ArmIO : public frc2::SubsystemBase {
52+
public:
53+
ArmIO(ArmHardwareInterface *hw_interface,
54+
ArmSoftwareInterface *sw_interface)
55+
: hw_interface_(hw_interface), sw_interface_(sw_interface) {}
56+
57+
/**
58+
* Will be called periodically whenever the CommandScheduler runs.
59+
*/
60+
void Periodic() override;
61+
62+
/**
63+
* Will be called periodically whenever the CommandScheduler runs during
64+
* simulation.
65+
*/
66+
void SimulationPeriodic() override;
67+
68+
bool ProcessIO();
69+
70+
private:
71+
bool UpdateArmConfig(ArmConfig &config);
72+
bool ResetEncoders();
73+
bool SetEncoder(double &val);
74+
75+
ArmHardwareInterface *const hw_interface_;
76+
ArmSoftwareInterface *const sw_interface_;
77+
};

‎src/main/include/subsystems/Arm.h‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
#pragma once
2+
3+
#include <frc/controller/PIDController.h>
4+
#include <frc2/command/SubsystemBase.h>
5+
#include <io/ArmIO.h>
6+
#include <memory>
7+
8+
class Arm : public frc2::SubsystemBase {
9+
public:
10+
Arm(ArmSoftwareInterface *interface) : interface_(interface) {}
11+
~Arm() {}
12+
13+
bool SetDegrees(double degrees);
14+
bool SetExtend(double inches);
15+
bool Init();
16+
void Periodic() override;
17+
18+
private:
19+
ArmSoftwareInterface *const interface_;
20+
std::shared_ptr<frc::PIDController> arm_pid_;
21+
std::shared_ptr<frc::PIDController> inches_pid_;
22+
double preset_;
23+
};

0 commit comments

Comments
 (0)