2017 Yakıt Fırlatıcı Robot
Bu örnekte, 2017 kazan hedefini uygulayan eksiksiz bir robot programı sunuyoruz. Bu programı 987 steamworks robotunda test ettik ve kazana kolayca yakıt atabildik. Bu örnekte, robotun tahrik sistemini kullanarak hedefleme ve menzile girme işlemlerini gerçekleştiriyoruz. Bize hemen belli olan bir şey, limelight'ın bu özelliği gerçek 2017 robotumuza kıyasla çok daha az kod ve zamanda uygulamamıza olanak sağladığıydı.
Aşağıdaki kodda LED'leri yanıp söndürme ve birden fazla görüş pipeline'ı arasında dinamik olarak geçiş yapma gibi birkaç başka özellik de bulunmaktadır. Bu örnek ayrıca atıcı tekerleğinin RPM'ini kontrol etmek için manyetik enkoder ile birlikte Talon SRX hız kontrolcüsünün nasıl kullanılacağını göstermektedir.
#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)
{
// Bu kod iki Talon'u birlikte çalışacak ve PID RPM kontrolü uygulayacak şekilde ayarlar
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); // Atıcı tekerleği için P,I,D,F sabitleri
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()
{
}
/*
* Operatör Kontrolü
*/
void OperatorControl() override
{
while (IsOperatorControl() && IsEnabled())
{
// Joystick'leri oku!
float left_command = m_stickLeft.GetY();
float right_command = m_stickRight.GetY();
// İzleme verilerini okumak için limelight tablosunu al
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);
// Kalibre edilmiş limelight artı işaretine göre hedefleme hatası ve mesafe hatası
float aim_error = targetX;
float dist_error = targetY;
// 0.2 derecelik ölü bant ile direksiyon ayarı (0.2 derecede yeterince yakın)
float steeringAdjust = KpAim*aim_error;
if (targetX > .2f) steeringAdjust += AimMinCmd;
else if (targetX < -.2f) steeringAdjust -= AimMinCmd;
// Mesafe ayarı, hedeften doğru mesafeye sür
float drivingAdjust = KpDist*dist_error;
bool doTarget = false;
if(m_stickLeft.GetRawButton(3)) // Pipeline 0 kullanarak hedefle
{
doTarget = true;
table->PutNumber("pipeline", 0);
}
else if (m_stickLeft.GetRawButton(2)) // Pipeline 1 kullanarak hedefle
{
doTarget = true;
table->PutNumber("pipeline", 1);
}
if(doTarget) // Otomatik hedefleme yapılıyorsa, sürüş ve direksiyonu ayarla
{
ShooterSetRPM(3300);
left_command += drivingAdjust - steeringAdjust;
right_command += drivingAdjust + steeringAdjust;
}
else
{
ShooterOff();
}
// Tank sürüşü, sol ve sağ tahrik motoru komutlarını gönder
StandardTank(left_command,right_command);
if(m_stickRight.GetRawButton(3)) // Topları em ve at
{
IntakeIn();
UptakeUp();
}
else if(m_stickRight.GetRawButton(2)) // Topları dışarı tükür
{
IntakeIn();
UptakeDown();
}
else // Topları rahat bırak!
{
IntakeOff();
UptakeOff();
}
if(m_stickLeft.GetRawButton(5)) // Joystick Düğmesi 5 = LED'leri yanıp söndür
{
table->PutNumber("ledMode", 2); // ışıkları yanıp söndür
}
else
{
table->PutNumber("ledMode", 0); // ışıkları aç
}
// motor güncelleme süresini bekle
frc::Wait(0.005);
}
}
/*
* Test modunda çalışır
*/
void Test() override {}
void StandardTank(float left, float right)
{
LeftDrive.Set(-left);
LeftDrive2.Set(-left);
RightDrive.Set(right);
RightDrive2.Set(right);
}
//
// Atıcı Fonksiyonları - atıcı tekerleği RPM'ini kontrol etmek için talon PID kullanır
// Talon'da P,I,D,F sabitlerini ayarlayın, bu değerler mekanizmanıza büyük ölçüde bağlıdır
//
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);
}
//
// Talon'lara istediğimiz atıcı tekerleği RPM'ini söyle
//
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);
}
//
// Sadece gücü ayarla -1..+1, RPM kontrolü ayarlandığından beri şu anda bunu kullanmıyoruz
//
void ShooterSetPower(float power)
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.Set(power);
Shooter.Set(power);
}
//
// Atıcı tekerleğini kapat
//
void ShooterOff()
{
Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
Shooter.Set(0.0f);
Shooter_2.Set(0.0f);
}
//
// Alım Fonksiyonları
//
void IntakeIn()
{
Intake.Set(-.8f);
}
void IntakeOut()
{
Intake.Set(.8f);
}
void IntakeShooting()
{
Intake.Set(-1.0f);
}
void IntakeOff()
{
Intake.Set(0);
}
//
// Yukarı Taşıma Fonksiyonları
//
void UptakeUp()
{
Uptake.Set(-1.0f);
}
void UptakeDown()
{
Uptake.Set(1.0f);
}
void UptakeOff()
{
Uptake.Set(0);
}
private:
// Robot tahrik sistemi
frc::Victor LeftDrive;
frc::Victor RightDrive;
frc::Victor LeftDrive2;
frc::Victor RightDrive2;
// atıcı tekerleği
CTRE::MotorControl::CAN::TalonSRX Shooter;
CTRE::MotorControl::CAN::TalonSRX Shooter_2;
CTRE::MotorControl::CAN::TalonSRX Intake;
CTRE::MotorControl::CAN::TalonSRX Uptake;
// Joystick girişleri
frc::Joystick m_stickLeft{0};
frc::Joystick m_stickRight{1};
};
START_ROBOT_CLASS(Robot)