2017 Yakıt Fırlatıcı Robot
Bu örnekte, 2017 kazan hedefini gerçekleştiren eksiksiz bir robot programı sunuyoruz. Bu programı 987 steamworks robotunda test ettik ve kazana kolayca yakıt atabildiğimizi gördük. Bu örnekte, robotun sürüş sistemini kullanarak hedefi nişan alıyor ve menzile giriyoruz. Hemen fark ettiğimiz şey, limelight'ın bu özelliği gerçek 2017 robotumuza kıyasla çok daha az kod ve zaman harcayarak uygulamamıza olanak sağlamasıydı.
Aşağıdaki kodda, LED'leri yanıp söndürme ve birden fazla görüş hattı arasında dinamik olarak geçiş yapma gibi başka özellikler de bulunmaktadır. Bu örnek ayrıca, atıcı tekerleğinin RPM'sini kontrol etmek için manyetik bir enkoder ile birlikte Talon SRX hız kontrolcüsünün nasıl kullanılacağını da 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ünü 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 nişangâhına dayalı nişan hatası ve mesafe hatası
float aim_error = targetX;
float dist_error = targetY;
// 0.2 derecelik bir ölü bölge 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ı, hedefe doğru mesafede sür
float drivingAdjust = KpDist*dist_error;
bool doTarget = false;
if(m_stickLeft.GetRawButton(3)) // Pipeline 0 kullanarak nişan al
{
doTarget = true;
table->PutNumber("pipeline", 0);
}
else if (m_stickLeft.GetRawButton(2)) // Pipeline 1 kullanarak nişan al
{
doTarget = true;
table->PutNumber("pipeline", 1);
}
if(doTarget) // Otomatik nişan alınıyorsa, sürüş ve yönlendirmeyi ayarla
{
ShooterSetRPM(3300);
left_command += drivingAdjust - steeringAdjust;
right_command += drivingAdjust + steeringAdjust;
}
else
{
ShooterOff();
}
// Tank sürüş, sol ve sağ sürüş sistemi motor komutlarını gönder
StandardTank(left_command,right_command);
if(m_stickRight.GetRawButton(3)) // Topları içeri çek ve at
{
IntakeIn();
UptakeUp();
}
else if(m_stickRight.GetRawButton(2)) // Topları dışarı at
{
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üresi için bekle
frc::Wait(0.005);
}
}
/*
* Test modu sırasında ç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'sini 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'sini 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ü -1..+1 arasında ayarla, şu anda RPM kontrolü ayarlandığından 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ıcı Fonksiyonları
//
void IntakeIn()
{
Intake.Set(-.8f);
}
void IntakeOut()
{
Intake.Set(.8f);
}
void IntakeShooting()
{
Intake.Set(-1.0f);
}
void IntakeOff()
{
Intake.Set(0);
}
//
// Yükseltici Fonksiyonları
//
void UptakeUp()
{
Uptake.Set(-1.0f);
}
void UptakeDown()
{
Uptake.Set(1.0f);
}
void UptakeOff()
{
Uptake.Set(0);
}
private:
// Robot sürüş 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)