Problem with sparkmax

[Gaspard]

Hello,
When we send a command to 2 sparkmax, first they started flashing green and red.
Then, after restarting the circuit, they started flashing orange/cyan.
We don’t understand why? It’s very annoying because when we send an instruction to the controller the motor does not do what is requested.

Thank you in advance for your reply

Gaspard team 5553 Robo’Lyon

[Fletch1373]

Please share a link to your code, ideally in github or similar.

[Gaspard]

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include <rev/CANSparkMax.h>
#include <frc/Joystick.h>
#include <frc/DutyCycleEncoder.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/DoubleSolenoid.h>
#include <frc/Compressor.h>

#define MAX_HEIGHT_CLIMBER 2.8
#define IS_CLIMBER_PID true

class Robot : public frc::TimedRobot
{
public:
void RobotInit() override;
void RobotPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;

private:
rev::CANSparkMax m_leftMotor{7, rev::CANSparkMax::MotorType::kBrushless};
rev::CANSparkMax m_leftMotorFollower{6, rev::CANSparkMax::MotorType::kBrushless};
rev::CANSparkMax m_rightMotor{1, rev::CANSparkMax::MotorType::kBrushless};
rev::CANSparkMax m_rightMotorFollower{4, rev::CANSparkMax::MotorType::kBrushless};
frc::Joystick m_joystickRight{0};
frc::Joystick m_joystickLeft{1};
frc::DutyCycleEncoder m_encoderRotatingArms{0};
frc::DutyCycleEncoder m_encoderClimber{9};
frc::DoubleSolenoid m_solenoidRotatingArms{frc::PneumaticsModuleType::CTREPCM, 1, 0};
frc::DoubleSolenoid m_solenoidClimber{frc::PneumaticsModuleType::CTREPCM, 3, 2};
frc::Compressor m_compressor{frc::PneumaticsModuleType::CTREPCM};
double time = 0;

double m_speedPID;
double m_speedCoef;
double m_setpoint;
double m_lastError;
double m_error;
double m_integrative;
double m_derivative;
double kP = 0.1;
double kI = 0.0001;
double kD = 0.001;
};

[Gaspard]

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include “Robot.h”

#include <frc/smartdashboard/SmartDashboard.h>
#include <spdlog/spdlog.h>

namespace utils
{
double Deadband(double value, double deadband = 0.1)
{
if (std::abs(value) < deadband)
{
return 0;
}

if (value >= 0)
{
  return (value - deadband) / (1.0 - deadband);
}
else
{
  return (value + deadband) / (1.0 - deadband);
}

}
} // namespace utils

void Robot::RobotInit()
{
m_leftMotor.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);
m_leftMotorFollower.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);
m_rightMotor.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);
m_rightMotorFollower.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);

// #if IS_CLIMBER_PID
// m_setpoint = 0.0;
// m_encoderClimber.Reset();
// m_integrative = 0.0;
// m_lastError = 0.0;
// frc::SmartDashboard::PutNumber(“setpoint”, 0.0);
// frc::SmartDashboard::PutNumber(“speedCoef”, 0.0);
// #endif

m_leftMotor.RestoreFactoryDefaults();
m_leftMotorFollower.RestoreFactoryDefaults();
m_rightMotor.RestoreFactoryDefaults();
m_rightMotorFollower.RestoreFactoryDefaults();

m_leftMotorFollower.Follow(m_leftMotor);
m_rightMotorFollower.Follow(m_rightMotor);

m_leftMotor.SetInverted(true);
m_leftMotorFollower.SetInverted(true);
m_rightMotor.SetInverted(false);
m_rightMotorFollower.SetInverted(false);
}

void Robot::RobotPeriodic()
{
}

void Robot::TeleopInit()
{
m_solenoidClimber.Set(frc::DoubleSolenoid::Value::kForward);
m_solenoidRotatingArms.Set(frc::DoubleSolenoid::Value::kForward);
}

/**

• This function is called periodically during operator control.
  */
  void Robot::TeleopPeriodic()
  {
  double offset = m_joystickLeft.GetThrottle() + m_joystickRight.GetThrottle() * 0.1;
  frc::SmartDashboard::PutNumber(“Offset”, offset);
  bool isAnyPTOEnabled = m_solenoidClimber.Get() == frc::DoubleSolenoid::Value::kReverse ||
  m_solenoidRotatingArms.Get() == frc::DoubleSolenoid::Value::kReverse;

double speedCoefficientLeft = m_solenoidClimber.Get() == frc::DoubleSolenoid::Value::kReverse ? 0.4 : 0.2;
double speedCoefficientRight = m_solenoidRotatingArms.Get() == frc::DoubleSolenoid::Value::kReverse ? 0.8 : 0.2;
double speedRight = utils::Deadband(-m_joystickRight.GetY(), 0.15) * speedCoefficientRight;
double speedLeft = utils::Deadband(-m_joystickLeft.GetY(), 0.15) * speedCoefficientLeft;
frc::SmartDashboard::PutNumber(“speedRight”, speedRight);
frc::SmartDashboard::PutNumber(“speedLeft”, speedLeft);

if (m_joystickLeft.GetRawButtonPressed(1))
{
frc::DoubleSolenoid::Value value = m_solenoidClimber.Get() == frc::DoubleSolenoid::Value::kForward ? frc::DoubleSolenoid::Value::kReverse : frc::DoubleSolenoid::Value::kForward;
m_solenoidClimber.Set(value);
}

if (m_joystickRight.GetRawButtonPressed(1))
{
frc::DoubleSolenoid::Value value = m_solenoidRotatingArms.Get() == frc::DoubleSolenoid::Value::kForward ? frc::DoubleSolenoid::Value::kReverse : frc::DoubleSolenoid::Value::kForward;
m_solenoidRotatingArms.Set(value);
}

if (m_joystickRight.GetRawButtonPressed(2))
{
if (m_compressor.Enabled())
{
m_compressor.Disable();
}
else
{
m_compressor.EnableDigital();
}
}

if (m_solenoidClimber.Get() == frc::DoubleSolenoid::Value::kReverse)
{
m_leftMotor.Set(speedLeft + offset);
}

if (m_solenoidRotatingArms.Get() == frc::DoubleSolenoid::Value::kReverse)
{
m_rightMotor.Set(-speedRight);
}

if (!isAnyPTOEnabled)
{
m_leftMotor.Set(speedLeft);
m_rightMotor.Set(speedRight);
}
if (m_joystickLeft.GetRawButton(6))
{
m_encoderClimber.Reset();
}

// #if IS_CLIMBER_PID
// frc::SmartDashboard::PutNumber(“encodeur climber”, m_encoderClimber.Get().value());
// frc::SmartDashboard::PutNumber(“error”, m_error);
// frc::SmartDashboard::PutNumber(“integrative”, m_integrative);
// frc::SmartDashboard::PutNumber(“derivative”, m_derivative);
// frc::SmartDashboard::PutNumber(“speedClimber_PID”, m_speedPID);
// m_speedCoef = frc::SmartDashboard::GetNumber(“speedCoef”, 0.0);
// m_setpoint = std::clamp(frc::SmartDashboard::GetNumber(“setpoint”, 0.0), 0.0, MAX_HEIGHT_CLIMBER);
// m_error = m_setpoint - m_encoderClimber.Get().value();
// m_integrative += (m_error * 0.02);
// m_derivative = (m_error - m_lastError) / .02;
// m_lastError = m_error;
// m_speedPID = std::abs(m_speedCoef) * std::clamp(kP * m_error + kI * m_integrative + kD * m_derivative, -1.0, 1.0);

// if (m_joystickLeft.GetRawButton(5))
// {
// m_leftMotor.Set(m_speedPID);
// }
// else
// {
// m_leftMotor.Set(0.0);
// }
// #endif
}

/**

• This function is called periodically during test mode.
  */
  void Robot::TestPeriodic() {}

#ifndef RUNNING_FRC_TESTS
int main()
{
return frc::StartRobot();
}
#endif