Robot Lanzador de Combustible 2017
En este ejemplo, presentamos un programa de robot completo que implementa el objetivo de la caldera de 2017. Probamos este programa en el robot steamworks del equipo 987 y pudimos anotar combustible fácilmente en la caldera. Para este ejemplo, apuntamos y nos movemos al rango usando el tren de transmisión del robot. Una cosa que fue inmediatamente evidente para nosotros es que limelight nos permitió implementar esta característica en una pequeña fracción de la cantidad de código y tiempo comparado con nuestro robot real de 2017.
También hay un par de otras características en el código a continuación, como la capacidad de hacer parpadear los LEDs y alternar dinámicamente entre múltiples pipelines de visión. Este ejemplo también muestra cómo usar un controlador de velocidad Talon SRX junto con un encoder magnético para controlar las RPM de la rueda del disparador.
#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 dos Talons para trabajar juntos e implementar control PID de RPM
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 la rueda del disparador
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()
{
}
/*
* Control del Operador
*/
void OperatorControl() override
{
while (IsOperatorControl() && IsEnabled())
{
// ¡Lee los joysticks!
float left_command = m_stickLeft.GetY();
float right_command = m_stickRight.GetY();
// Obtener tabla de limelight para leer datos de seguimiento
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);
// Error de apuntado y error de distancia basado en la cruz calibrada de limelight
float aim_error = targetX;
float dist_error = targetY;
// Ajuste de dirección con una banda muerta de 0.2 grados (suficientemente cerca a 0.2 grados)
float steeringAdjust = KpAim*aim_error;
if (targetX > .2f) steeringAdjust += AimMinCmd;
else if (targetX < -.2f) steeringAdjust -= AimMinCmd;
// Ajuste de distancia, conducir a la distancia correcta del objetivo
float drivingAdjust = KpDist*dist_error;
bool doTarget = false;
if(m_stickLeft.GetRawButton(3)) // Apuntar usando pipeline 0
{
doTarget = true;
table->PutNumber("pipeline", 0);
}
else if (m_stickLeft.GetRawButton(2)) // Apuntar usando pipeline 1
{
doTarget = true;
table->PutNumber("pipeline", 1);
}
if(doTarget) // Si está auto-apuntando, ajustar conducción y dirección
{
ShooterSetRPM(3300);
left_command += drivingAdjust - steeringAdjust;
right_command += drivingAdjust + steeringAdjust;
}
else
{
ShooterOff();
}
// Conducción tipo tanque, enviar comandos de motor izquierdo y derecho del tren de transmisión
StandardTank(left_command,right_command);
if(m_stickRight.GetRawButton(3)) // Succionar y disparar bolas
{
IntakeIn();
UptakeUp();
}
else if(m_stickRight.GetRawButton(2)) // Escupir bolas
{
IntakeIn();
UptakeDown();
}
else // ¡Dejar las bolas en paz!
{
IntakeOff();
UptakeOff();
}
if(m_stickLeft.GetRawButton(5)) // Botón 5 del Joystick = Hacer parpadear los LEDs
{
table->PutNumber("ledMode", 2); // hacer parpadear las luces
}
else
{
table->PutNumber("ledMode", 0); // encender las luces
}
// esperar un tiempo de actualización del motor
frc::Wait(0.005);
}
}
/*
* Se ejecuta durante el modo de prueba
*/
void Test() override {}
void StandardTank(float left, float right)
{
LeftDrive.Set(-left);
LeftDrive2.Set(-left);
RightDrive.Set(right);
RightDrive2.Set(right);
}
//
// Funciones del Disparador - usa PID del talon para controlar las RPM de la rueda del disparador
// Establecer las constantes P,I,D,F en el Talon, estos valores dependen mucho de tu 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);
}
//
// Decirle a los talons nuestras RPM deseadas de la rueda del disparador
//
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);
}
//
// Solo establecer la potencia -1..+1, no se usa actualmente ahora que el control 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);
}
//
// Apagar la rueda del disparador
//
void ShooterOff()
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter.Set(0.0f);
Shooter_2.Set(0.0f);
}
//
// Funciones de Intake
//
void IntakeIn()
{
Intake.Set(-.8f);
}
void IntakeOut()
{
Intake.Set(.8f);
}
void IntakeShooting()
{
Intake.Set(-1.0f);
}
void IntakeOff()
{
Intake.Set(0);
}
//
// Funciones de Uptake
//
void UptakeUp()
{
Uptake.Set(-1.0f);
}
void UptakeDown()
{
Uptake.Set(1.0f);
}
void UptakeOff()
{
Uptake.Set(0);
}
private:
// Sistema de conducción del robot
frc::Victor LeftDrive;
frc::Victor RightDrive;
frc::Victor LeftDrive2;
frc::Victor RightDrive2;
// rueda del disparador
CTRE::MotorControl::CAN::TalonSRX Shooter;
CTRE::MotorControl::CAN::TalonSRX Shooter_2;
CTRE::MotorControl::CAN::TalonSRX Intake;
CTRE::MotorControl::CAN::TalonSRX Uptake;
// Entradas de joystick
frc::Joystick m_stickLeft{0};
frc::Joystick m_stickRight{1};
};
START_ROBOT_CLASS(Robot)