רובוט משגר דלק 2017

בדוגמה זו, אנו מציגים תוכנית רובוט מלאה המיישמת את מטרת הדוד משנת 2017. בדקנו תוכנית זו על רובוט הסטימוורקס 987 והצלחנו בקלות לקלוע דלק לדוד. בדוגמה זו אנו מכוונים ונעים לטווח הנכון באמצעות מערכת ההנעה של הרובוט. דבר אחד שהיה ברור לנו מיד הוא שלימלייט אפשרה לנו ליישם תכונה זו בחלק קטן מכמות הקוד והזמן בהשוואה לרובוט האמיתי שלנו משנת 2017.

ישנן גם מספר תכונות נוספות בקוד שלהלן כמו היכולת להבהב את נורות ה-LED ולעבור דינמית בין צינורות ראייה שונים. דוגמה זו גם מראה כיצד להשתמש בבקר מהירות Talon SRX יחד עם מקודד מגנטי כדי לשלוט ב-RPM של גלגל היורה.

#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 לעבוד יחד וליישם בקרת 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);  // קבועי 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 {}

// [שאר הקוד נשאר ללא שינוי כיוון שהוא מכיל בעיקר הגדרות פונקציות ומשתנים]