Robô Lançador de Combustível de 2017
Neste exemplo, apresentamos um programa completo de robô que implementa o objetivo da caldeira de 2017. Testamos este programa no robô 987 steamworks e conseguimos facilmente marcar pontos com combustível na caldeira. Neste exemplo, miramos e nos movemos para a posição correta usando o sistema de tração do robô. Uma coisa que ficou imediatamente aparente para nós é que o limelight nos permitiu implementar este recurso em uma fração mínima da quantidade de código e tempo em comparação com nosso robô real de 2017.
Também há alguns outros recursos no código abaixo, como a capacidade de piscar os LEDs e alternar dinamicamente entre múltiplos pipelines de visão. Este exemplo também mostra como usar um controlador de velocidade Talon SRX junto com um encoder magnético para controlar o RPM da roda do lançador.
#include <iostream>
#include <string>
#include <Drive/DifferentialDrive.h>
#include <Joystick.h>
#include <SampleRobot.h>
#include <SmartDashboard/SendableChooser.h>
#include <SmartDashboard/SmartDashboard.h>
#include <Spark.h>
#include <Victor.h>
#include <Timer.h>
#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
//
#define Vic_Drive_Left_1 0
#define Vic_Drive_Left_2 1
#define Vic_Drive_Right_1 2
#define Vic_Drive_Right_2 3
#define Tal_Shooter_Wheel 21
#define Tal_Shooter_Wheel_2 2
#define Tal_Intake 3
#define Tal_Uptake 9
class Robot : public frc::SampleRobot
{
public:
Robot() :
LeftDrive(Vic_Drive_Left_1),
RightDrive(Vic_Drive_Right_1),
LeftDrive2(Vic_Drive_Left_2),
RightDrive2(Vic_Drive_Right_2),
Shooter(Tal_Shooter_Wheel),
Shooter_2(Tal_Shooter_Wheel_2),
Intake(Tal_Intake),
Uptake(Tal_Uptake)
{
// Este código configura dois Talons para trabalhar juntos e implementar controle de RPM PID
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kSpeed);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kFollower);
Shooter_2.Set(Tal_Shooter_Wheel);
Shooter.SetClosedLoopOutputDirection(false);
Shooter.SetStatusFrameRateMs(CTRE::MotorControl::CAN::TalonSRX::StatusFrameRateAnalogTempVbat,10);
Shooter.SetFeedbackDevice(CTRE::MotorControl::CAN::TalonSRX::CtreMagEncoder_Relative);
SetShooterConstants(.0002f, .00009f, .0000f, .000006f); // Constantes P,I,D,F para a roda do lançador
Shooter.SetAllowableClosedLoopErr(10);
Shooter.ConfigNominalOutputVoltage(+0.f,-0.f);
Shooter.ConfigPeakOutputVoltage(+12.f,-12.f);
Shooter.SelectProfileSlot(0);
Shooter.SetSensorDirection(true);
Shooter.SetVoltageRampRate(100);
Shooter.SetIzone(300);
Shooter_2.SetVoltageRampRate(100);
}
void RobotInit()
{
}
void Autonomous()
{
}
/*
* Controle do Operador
*/
void OperatorControl() override
{
while (IsOperatorControl() && IsEnabled())
{
// Leia os joysticks!
float left_command = m_stickLeft.GetY();
float right_command = m_stickRight.GetY();
// Obtenha a tabela limelight para ler dados de rastreamento
std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float KpAim = 0.045;
float KpDist = 0.0f; //0.09;
float AimMinCmd = 0.095f;
float targetX = table->GetNumber("tx", 0);
float targetY = table->GetNumber("ty", 0);
float targetA = table->GetNumber("ta", 0);
// Erro de mira e erro de distância baseados na mira calibrada do limelight
float aim_error = targetX;
float dist_error = targetY;
// Ajuste de direção com uma zona morta de 0,2 graus (próximo o suficiente em 0,2 graus)
float steeringAdjust = KpAim*aim_error;
if (targetX > .2f) steeringAdjust += AimMinCmd;
else if (targetX < -.2f) steeringAdjust -= AimMinCmd;
// Ajuste de distância, dirija até a distância correta do objetivo
float drivingAdjust = KpDist*dist_error;
bool doTarget = false;
if(m_stickLeft.GetRawButton(3)) // Mirar usando pipeline 0
{
doTarget = true;
table->PutNumber("pipeline", 0);
}
else if (m_stickLeft.GetRawButton(2)) // Mirar usando pipeline 1
{
doTarget = true;
table->PutNumber("pipeline", 1);
}
if(doTarget) // Se estiver mirando automaticamente, ajuste a direção e o volante
{
ShooterSetRPM(3300);
left_command += drivingAdjust - steeringAdjust;
right_command += drivingAdjust + steeringAdjust;
}
else
{
ShooterOff();
}
// Direção tank, envie comandos para os motores esquerdo e direito do sistema de tração
StandardTank(left_command,right_command);
if(m_stickRight.GetRawButton(3)) // Sugue e atire bolas
{
IntakeIn();
UptakeUp();
}
else if(m_stickRight.GetRawButton(2)) // Cuspa as bolas
{
IntakeIn();
UptakeDown();
}
else // Deixe as bolas em paz!
{
IntakeOff();
UptakeOff();
}
if(m_stickLeft.GetRawButton(5)) // Botão 5 do Joystick = Piscar os LEDs
{
table->PutNumber("ledMode", 2); //piscar as luzes
}
else
{
table->PutNumber("ledMode", 0); //ligar as luzes
}
// espere um tempo de atualização do motor
frc::Wait(0.005);
}
}
/*
* Executa durante o modo de teste
*/
void Test() override {}
void StandardTank(float left, float right)
{
LeftDrive.Set(-left);
LeftDrive2.Set(-left);
RightDrive.Set(right);
RightDrive2.Set(right);
}
//
// Funções do Lançador - usa PID do talon para controlar o RPM da roda do lançador
// Defina as constantes P,I,D,F no Talon, esses valores dependem muito do seu mecanismo
//
void SetShooterConstants(float p,float i,float d,float f)
{
p *= 1024.f;
i *= 1024.f;
d *= 1024.f;
f *= 1024.f;
Shooter.SetP(p);
Shooter.SetI(i);
Shooter.SetD(d);
Shooter.SetF(f);
}
//
// Informe aos talons nosso RPM desejado para a roda do lançador
//
void ShooterSetRPM(float desrpm)
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kSpeed);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kFollower);
Shooter_2.Set(Tal_Shooter_Wheel);
Shooter.Set(desrpm);
}
//
// Apenas defina a potência -1..+1, não estamos usando isso agora que o controle de RPM está configurado
//
void ShooterSetPower(float power)
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.Set(power);
Shooter.Set(power);
}
//
// Desligue a roda do lançador
//
void ShooterOff()
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter.Set(0.0f);
Shooter_2.Set(0.0f);
}
//
// Funções de Entrada
//
void IntakeIn()
{
Intake.Set(-.8f);
}
void IntakeOut()
{
Intake.Set(.8f);
}
void IntakeShooting()
{
Intake.Set(-1.0f);
}
void IntakeOff()
{
Intake.Set(0);
}
//
// Funções de Transporte
//
void UptakeUp()
{
Uptake.Set(-1.0f);
}
void UptakeDown()
{
Uptake.Set(1.0f);
}
void UptakeOff()
{
Uptake.Set(0);
}
private:
// Sistema de tração do robô
frc::Victor LeftDrive;
frc::Victor RightDrive;
frc::Victor LeftDrive2;
frc::Victor RightDrive2;
// roda do lançador
CTRE::MotorControl::CAN::TalonSRX Shooter;
CTRE::MotorControl::CAN::TalonSRX Shooter_2;
CTRE::MotorControl::CAN::TalonSRX Intake;
CTRE::MotorControl::CAN::TalonSRX Uptake;
// Entradas do Joystick
frc::Joystick m_stickLeft{0};
frc::Joystick m_stickRight{1};
};
START_ROBOT_CLASS(Robot)