added Manual Arm Command and fixed errors (#39)

let's go!

* added Manual Arm Command and fixed errors

* added one of the relative encoders for the arm motors, and made ProcessIO actually get called in ArmIO

* added logging for part of the arm

* fixed joystick stuff for second driver

* added relative encoder to Sensors

* arm logs (the oldest one should be with additional weight, the others are bare)

---------

Co-authored-by: okcroboticsteam <okcroboticsteam@gmail.com>
Co-authored-by: danielbrownmsm <daniel.brown@msmrockets.org>

Author: 3 people

logs/good logs/FRC_20230205_212714.wpilog

@@ -10,6 +10,9 @@ RobotContainer::RobotContainer() {
     VOKC_CALL(this->InitHardware(hardware_));
 
     VOKC_CALL(this->InitSwerve());
+    VOKC_CALL(this->InitArm());
+
+    
 
     // Initialize the Gamepads
     VOKC_CALL(InitGamepads());
@@ -67,6 +70,9 @@ bool RobotContainer::InitActuators(Actuators *actuators_interface) {
     actuators_interface->right_front_steer_motor = std::make_unique<rev::CANSparkMax>(RIGHT_FRONT_STEER_MOTOR, BRUSHLESS);
     actuators_interface->right_back_steer_motor = std::make_unique<rev::CANSparkMax>(RIGHT_BACK_STEER_MOTOR, BRUSHLESS);
 
+    actuators_interface->arm_lift_motor = std::make_unique<rev::CANSparkMax>(ARM_LIFT_MOTOR, BRUSHLESS);
+    actuators_interface->arm_up_motor = std::make_unique<rev::CANSparkMax>(ARM_UP_MOTOR, BRUSHLESS);
+    actuators_interface->arm_extend_motor = std::make_unique<rev::CANSparkMax>(ARM_EXTEND_MOTOR, BRUSHLESS);
     return true;
 }
 
@@ -123,6 +129,10 @@ bool RobotContainer::InitSensors(const Actuators &actuators,
     OKC_CHECK(sensor_interface->right_front_steer_encoder != nullptr);
     OKC_CHECK(sensor_interface->right_back_steer_encoder != nullptr);
 
+    sensor_interface->arm_lift_encoder = std::make_unique<rev::SparkMaxRelativeEncoder>(actuators.arm_lift_motor->GetEncoder());
+
+    OKC_CHECK(sensor_interface->arm_lift_encoder != nullptr);
+
     return true;
 }
 
@@ -144,11 +154,28 @@ bool RobotContainer::InitSwerve() {
     return true;
 }
 
+bool RobotContainer::InitArm() {
+    OKC_CALL(SetupArmInterface(hardware_, arm_hw_));
+
+    arm_sw_ = std::make_shared<ArmSoftwareInterface>();
+
+    arm_io_ = std::make_shared<ArmIO>(arm_hw_.get(), arm_sw_.get());
+    
+    arm_ = std::make_shared<Arm>(arm_sw_.get());
+
+    OKC_CALL(arm_->Init());
+
+    return true;
+}
+
 bool RobotContainer::InitGamepads() {
     // Get joystick IDs from parameters.toml
     int gamepad1_id = RobotParams::GetParam("gamepad1_id", 0);
+    int gamepad2_id = RobotParams::GetParam("gamepad2_id", 1);
 
     gamepad1_ = std::make_shared<frc::Joystick>(gamepad1_id);
+    gamepad2_ = std::make_shared<frc::Joystick>(gamepad2_id);
+
 
     // Initialize the joystick buttons
     driver_a_button_ =
@@ -170,5 +197,8 @@ bool RobotContainer::InitCommands() {
 
     swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_);
 
+    manual_arm_command_ =std::make_shared<ManualArmCommand>(arm_, gamepad2_);
+    arm_->SetDefaultCommand(*manual_arm_command_);
+
     return true;
 }

logs/good logs/FRC_20230205_213747.wpilog

@@ -0,0 +1,41 @@
+#include "commands/arm/ManualArmCommand.h"
+
+ManualArmCommand::ManualArmCommand(std::shared_ptr<Arm> arm, std::shared_ptr<frc::Joystick> gamepad) {
+    arm_ = arm;
+    gamepad_ = gamepad;
+
+    if (arm_ != nullptr) {
+        this->AddRequirements(arm_.get());
+    }
+}
+
+void ManualArmCommand::Initialize() {
+    VOKC_CHECK(arm_ != nullptr);
+
+    arm_->SetControlMode(Manual);
+}
+
+void ManualArmCommand::Execute() {
+    VOKC_CHECK(arm_ != nullptr);
+    VOKC_CHECK(gamepad_ != nullptr);
+
+   
+    double lift_power_ = this->gamepad_->GetRawAxis(1);
+    double up_power_ = this->gamepad_->GetRawAxis(0);
+    double extend_power_ = this->gamepad_->GetRawAxis(2);
+
+    VOKC_CALL(arm_->SetManualLiftPower(lift_power_));
+    VOKC_CALL(arm_->SetManualUpPower(-lift_power_));
+    // VOKC_CALL(arm_->SetManualExtendPower(extend_power_));
+
+    
+}
+
+void ManualArmCommand::End(bool interrupted) {
+    // do nothing lol
+}
+
+bool ManualArmCommand::IsFinished() {
+    // this command should never end, unless it is interrupted by another
+    return false;
+}

logs/good logs/FRC_20230205_214207.wpilog

@@ -62,4 +62,30 @@ bool SetupSwerveDriveInterface(
     interface = std::make_shared<SwerveDriveHardwareInterface>(swerve_drive_interface);
 
     return true;
+}
+ bool SetupArmInterface(std::unique_ptr<Hardware> &hardware,
+                            std::shared_ptr<ArmHardwareInterface> &interface){ 
+ OKC_CHECK(hardware->actuators != nullptr);
+                            
+
+    //Get actuators interface for arm
+    std::unique_ptr<Actuators> &actuators = hardware->actuators;
+    std::unique_ptr<Sensors> &sensors = hardware->sensors;
+    
+   //make sure the actuators actually exist 
+   OKC_CHECK(actuators->arm_lift_motor != nullptr);
+   OKC_CHECK(actuators->arm_up_motor != nullptr);
+   OKC_CHECK(actuators->arm_extend_motor != nullptr);
+
+   ArmHardwareInterface arm_interface = {actuators->arm_lift_motor.get(),
+                                         actuators->arm_up_motor.get(),
+                                         actuators->arm_extend_motor.get(),
+                                         sensors->arm_lift_encoder.get()
+                                         };
+    interface = std::make_shared<ArmHardwareInterface>(arm_interface);
+
+    return true;
+
+
+
 }

src/main/cpp/RobotContainer.cpp

@@ -3,6 +3,7 @@
 
 void ArmIO::Periodic() {
     // Process all the inputs and outputs to/from high level software.
+    
     VOKC_CALL(ProcessIO());
 }
 
@@ -31,23 +32,23 @@ bool ArmIO::ProcessIO() {
         sw_interface_->reset_encoders = false;
     }
 
-    // if the encoder should be set to a specific value
-    if (sw_interface_->set_encoder_to_val) {
-        OKC_CALL(SetEncoder(sw_interface_->encoder_val_to_set));
-
-        sw_interface_->set_encoder_to_val = false;
-    }
+    
 
     // Get the hardware sensor values.
     // limit switches
-    sw_interface_->deployed_limit_switch_val =
-        hw_interface_->deploy_limit_switch->Get(); //???
-    sw_interface_->retracted_limit_switch_val =
-        hw_interface_->retracted_limit_switch->Get(); //???
+    // sw_interface_->deployed_limit_switch_val =
+    //     hw_interface_->deploy_limit_switch->Get(); //???
+    // sw_interface_->retracted_limit_switch_val =
+    //     hw_interface_->retracted_limit_switch->Get(); //???
 
     // intake position encoder
-    sw_interface_->arm_position_encoder_val =
-        hw_interface_->arm_position_motor->GetEncoder().GetPosition();
+    OKC_CHECK(hw_interface_->arm_extend_motor != nullptr);
+    OKC_CHECK(hw_interface_->arm_up_motor != nullptr);
+    hw_interface_->arm_lift_motor->Set(sw_interface_->arm_lift_power);
+    hw_interface_->arm_up_motor->Set(sw_interface_->arm_up_power);
+    hw_interface_->arm_extend_motor->Set(sw_interface_->arm_extend_power);
+
+    sw_interface_->arm_lift_encoder_val = hw_interface_->arm_lift_encoder->GetPosition();
 
     return true;
 }
@@ -64,7 +65,8 @@ bool ArmIO::UpdateArmConfig(ArmConfig &config) {
     // Apply the configuration
     // Open Loop Ramp Rate
     hw_interface_->arm_lift_motor->SetOpenLoopRampRate(open_loop_ramp);
-    hw_interface_->indexer_motor->SetOpenLoopRampRate(open_loop_ramp);
+    hw_interface_->arm_up_motor->SetOpenLoopRampRate(open_loop_ramp);
+    hw_interface_->arm_extend_motor->SetOpenLoopRampRate(open_loop_ramp);
 
     // Java code has this commented out as well, I'm assuming because it was
     // meesing something up. well, the intake works (probably, been a while
@@ -73,15 +75,19 @@ bool ArmIO::UpdateArmConfig(ArmConfig &config) {
     // hw_interface_->intake_position_motor->SetOpenLoopRampRate(open_loop_ramp);
 
     // current limiting, so the neo 550 on the indexer doesn't stall and smoke
-    hw_interface_->indexer_motor->SetSmartCurrentLimit(max_indexer_current);
+    hw_interface_->arm_lift_motor->SetSmartCurrentLimit(max_indexer_current);
+    hw_interface_->arm_up_motor->SetSmartCurrentLimit(max_indexer_current);
+    hw_interface_->arm_extend_motor->SetSmartCurrentLimit(max_indexer_current);
 
     return true;
 }
-
+//HACK: this is a hack
 bool ArmIO::ResetEncoders() {
     OKC_CHECK(hw_interface_ != nullptr);
 
-    hw_interface_->arm_position_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->arm_lift_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->arm_up_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->arm_extend_motor->GetEncoder().SetPosition(0.0);
 
     // we currently don't use the encoder for these other two motors, so we
     // don't need to reset them yet. yet.
@@ -91,10 +97,3 @@ bool ArmIO::ResetEncoders() {
     return true;
 }
 
-bool ArmIO::SetEncoder(double &val) {
-    OKC_CHECK(hw_interface_ != nullptr);
-
-    hw_interface_->arm_position_motor->GetEncoder().SetPosition(val);
-
-    return true;
-}

src/main/cpp/commands/arm/ManualArmCommand.cpp

@@ -4,9 +4,17 @@ bool Arm::Init() {
     arm_pid_ = std::make_shared<frc::PIDController>(0, 0, 0);
 
     inches_pid_ = std::make_shared<frc::PIDController>(0, 0, 0);
+
+    arm_lift_output_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/arm/lift_output");
+    arm_lift_enc_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/arm/lift_enc");
+
     return true;
 }
+bool Arm::SetControlMode(const ArmMode &mode){
+    mode_= mode;
 
+    return true;
+}
 bool Arm::SetDegrees(double degrees) {
     this->arm_pid_->SetSetpoint(degrees);
 
@@ -19,11 +27,54 @@ bool Arm::SetExtend(double inches) {
     return true;
 }
 
+bool Arm::SetManualLiftPower(double power)
+{
+    lift_power_ = power;
+
+    return true;
+}
+
+bool Arm::SetManualExtendPower(double power)
+{
+    extend_power_ = power;
+
+    return true;
+}
+
+bool Arm::ManualControl() {
+    OKC_CHECK(interface_ != nullptr);
+
+    interface_->arm_lift_power = lift_power_;
+    interface_->arm_up_power = up_power_;
+    interface_->arm_extend_power = extend_power_;
+
+    arm_lift_output_log_.Append(interface_->arm_lift_power);
+    arm_lift_enc_log_.Append(interface_->arm_lift_encoder_val);
+
+
+    return true;
+}
+bool Arm::SetManualUpPower(double power)
+{
+    up_power_ = power;
+
+    return true;
+
+}
+
 void Arm::Periodic() {
-    this->interface_->arm_lift_power =
-        this->arm_pid_->Calculate(this->interface_->arm_encoder);
-    this->interface_->arm_extend_power =
-        this->inches_pid_->Calculate(this->interface_->arm_extend_encoder);
+        
+    switch (mode_) {
+        case Manual:
+            VOKC_CALL(this->ManualControl());
+            break;
+        case Auto:
+            //this->interface_->arm_lift_power = this->arm_pid_->Caluculate(this->interface_->arm_encoder);
+            //this->interface_->arm_up_power = this->arm_pid_->Caluculate(this->interface_->arm_extend_encoder);
+            break;
+        default:
+            VOKC_CHECK_MSG(false, "Unhandled enum")    
+    }
 }
 
 // TODO: Understand this example code and implement it correctly

src/main/cpp/hardware/Hardware.cpp

@@ -4,7 +4,7 @@ competition = false
 
 [ui]
 gamepad1_id = 0
-gamepad2_id = 2
+gamepad2_id = 1
 
 [robot]
 neo_550_ticks_per_rev = 42

src/main/cpp/io/ArmIO.cpp

@@ -7,8 +7,17 @@
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
+
 #include "RobotContainer.h"
 
+
+ //Arm hardware 
+
+
+
+
+
+
 class Robot : public frc::TimedRobot {
 public:
     void RobotInit() override;