روبوت قاذف الوقود لعام 2017

في هذا المثال، نقدم برنامجًا كاملاً للروبوت يطبق هدف الغلاية لعام 2017. لقد اختبرنا هذا البرنامج على روبوت steamworks 987 وتمكنا بسهولة من تسجيل الوقود في الغلاية. في هذا المثال، نقوم بالتصويب والتحرك إلى النطاق المناسب باستخدام نظام القيادة للروبوت. أحد الأمور التي كانت واضحة لنا على الفور هو أن Limelight سمح لنا بتنفيذ هذه الميزة بجزء صغير من كمية الشفرة والوقت مقارنة بروبوتنا الحقيقي لعام 2017.

هناك أيضًا بعض الميزات الأخرى في الكود أدناه مثل القدرة على وميض مصابيح LED والتبديل الديناميكي بين خطوط الرؤية المتعددة. يوضح هذا المثال أيضًا كيفية استخدام وحدة تحكم السرعة Talon SRX مع جهاز تشفير مغناطيسي للتحكم في دورات في الدقيقة لعجلة القاذف.

#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)
{
        // يقوم هذا الكود بإعداد اثنين من Talons للعمل معًا وتنفيذ التحكم في دورات في الدقيقة باستخدام 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);  // ثوابت P,I,D,F لعجلة القاذف
        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()
{
}

/*
 * التحكم بواسطة المشغل
 */
void OperatorControl() override
{
        while (IsOperatorControl() && IsEnabled())
        {
                // قراءة عصي التحكم!
                float left_command = m_stickLeft.GetY();
                float right_command = m_stickRight.GetY();

                // الحصول على جدول limelight لقراءة بيانات التتبع
                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);

                // خطأ التصويب وخطأ المسافة بناءً على الشعرية المتقاطعة المعايرة لـ limelight
                float aim_error = targetX;
                float dist_error = targetY;

                // تعديل التوجيه مع نطاق ميت 0.2 درجة (قريب بما يكفي عند 0.2 درجة)
                float steeringAdjust = KpAim*aim_error;
                if (targetX > .2f) steeringAdjust += AimMinCmd;
                else if (targetX < -.2f) steeringAdjust -= AimMinCmd;

                // تعديل المسافة، القيادة إلى المسافة الصحيحة من الهدف
                float drivingAdjust = KpDist*dist_error;
                bool doTarget = false;

                if(m_stickLeft.GetRawButton(3))         // التصويب باستخدام خط الرؤية 0
                {
                        doTarget = true;
                        table->PutNumber("pipeline", 0);
                }
                else if (m_stickLeft.GetRawButton(2))   // التصويب باستخدام خط الرؤية 1
                {
                        doTarget = true;
                        table->PutNumber("pipeline", 1);
                }

                if(doTarget)                            // إذا كان التصويب التلقائي نشطًا، قم بتعديل القيادة والتوجيه
                {
                        ShooterSetRPM(3300);
                        left_command += drivingAdjust - steeringAdjust;
                        right_command += drivingAdjust + steeringAdjust;
                }
                else
                {
                        ShooterOff();
                }

                // قيادة الدبابة، إرسال أوامر المحرك الأيسر والأيمن لنظام القيادة
                StandardTank(left_command,right_command);

                if(m_stickRight.GetRawButton(3))        // شفط وإطلاق الكرات
                {
                        IntakeIn();
                        UptakeUp();
                }
                else if(m_stickRight.GetRawButton(2))   // بصق الكرات
                {
                        IntakeIn();
                        UptakeDown();
                }
                else                                    // اترك الكرات وشأنها!
                {
                        IntakeOff();
                        UptakeOff();
                }
                if(m_stickLeft.GetRawButton(5))         // زر عصا التحكم 5 = وميض مصابيح LED
                {
                        table->PutNumber("ledMode", 2); // وميض الأضواء
                }
                else
                {
                        table->PutNumber("ledMode", 0); // تشغيل الأضواء
                }

                // انتظار وقت تحديث المحرك
                frc::Wait(0.005);
        }
}

/*
 * يعمل أثناء وضع الاختبار
 */
void Test() override {}


void StandardTank(float left, float right)
{
        LeftDrive.Set(-left);
        LeftDrive2.Set(-left);
        RightDrive.Set(right);
        RightDrive2.Set(right);
}

//
// وظائف القاذف - تستخدم PID Talon للتحكم في دورات في الدقيقة لعجلة القاذف
// قم بتعيين ثوابت P,I,D,F في Talon، تعتمد هذه القيم بشكل كبير على آليتك
//
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);
}

//
// أخبر Talons بدورات في الدقيقة المطلوبة لعجلة القاذف
//
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);
}

//
// قم بتعيين الطاقة -1..+1 فقط، لا نستخدمها حاليًا الآن بعد إعداد التحكم في دورات في الدقيقة
//
void ShooterSetPower(float power)
{
        Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
        Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
        Shooter_2.Set(power);
        Shooter.Set(power);
}

//
// إيقاف تشغيل عجلة القاذف
//
void ShooterOff()
{
        Shooter.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
        Shooter_2.SetControlMode(CTRE::MotorControl::ControlMode::kPercentVbus);
        Shooter.Set(0.0f);
        Shooter_2.Set(0.0f);
}

//
// وظائف الإدخال
//
void IntakeIn()
{
        Intake.Set(-.8f);
}
void IntakeOut()
{
        Intake.Set(.8f);
}
void IntakeShooting()
{
        Intake.Set(-1.0f);
}

void IntakeOff()
{
        Intake.Set(0);
}

//
// وظائف الرفع
//
void UptakeUp()
{
        Uptake.Set(-1.0f);
}
void UptakeDown()
{
        Uptake.Set(1.0f);
}
void UptakeOff()
{
        Uptake.Set(0);
}

private:

        // نظام قيادة الروبوت
        frc::Victor LeftDrive;
        frc::Victor RightDrive;
        frc::Victor LeftDrive2;
        frc::Victor RightDrive2;

        // عجلة القاذف
        CTRE::MotorControl::CAN::TalonSRX Shooter;
        CTRE::MotorControl::CAN::TalonSRX Shooter_2;
        CTRE::MotorControl::CAN::TalonSRX Intake;
        CTRE::MotorControl::CAN::TalonSRX Uptake;

        // مدخلات عصا التحكم
        frc::Joystick m_stickLeft{0};
        frc::Joystick m_stickRight{1};
};

START_ROBOT_CLASS(Robot)