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

בדוגמה זו, אנו מציגים תוכנית רובוט מלאה המיישמת את מטרת הדוד של 2017. בדקנו תוכנית זו על רובוט ה-steamworks של קבוצה 987 והצלחנו בקלות להבקיע דלק בדוד. בדוגמה זו אנו מכוונים ונעים לטווח באמצעות מערכת ההנעה של הרובוט. דבר אחד שהיה ברור לנו מיד הוא ש-limelight אפשר לנו ליישם תכונה זו בחלק קטן מאוד מכמות הקוד והזמן בהשוואה לרובוט האמיתי שלנו מ-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 {}


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

//
// פונקציות ירי - משתמש ב-PID של talon לשליטה ב-RPM של גלגל הירי
// הגדר את קבועי 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 את ה-RPM הרצוי של גלגל הירי
//
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, לא משתמשים בזה כרגע עכשיו שבקרת RPM מוגדרת
//
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)