##### tags:`Limelight Code`

FRC 程式
===
---
## Limelight
1.在2024的比賽中有16個Apriltag，顯然地不可能每個按鈕都單獨拉一個AprilTag寫，按鈕不可能這麼多，實際上就算真的寫了16個按鈕Driver也記不起來，所以我們要想一個簡略的判斷方式以方便我們維護程式跟除錯。
2.因為這季沒有要用trap所以stage的Apriltag是可以不用對的，也就是說我們可以只需要對Amp,Source,Speaker就好，那這樣一個簡易的想法就出來了: **我們的判斷式主要有兩層，第一層式判斷紅藍方,第二層是判斷目標名稱**，那麼一個簡易的判斷想法就出來了

 if(getTagID() == 7 || getTagID() == 4){
            TagName = "Speaker";
        }
        else if(getTagID() == 5 || getTagID() == 6){
            TagName = "Amp";
        }
        else if(getTagID() == 1 || getTagID() == 2 || getTagID() == 9 || getTagID() == 10){
            TagName = "LoadingStation";
        }
        if(getTagID() == 3 || getTagID() == 4 || getTagID() == 5 || getTagID() == 9 || getTagID() == 10){
            TagAlliance = "Red";
        }
        else if(getTagID() == 1 || getTagID() == 2 || getTagID() == 6 || getTagID() == 7 || getTagID() == 8){
            TagAlliance = "Blue";
        }

但要是這樣就直接丟給```Comand```的話整個判斷就太醜了，因此我們需要一個```Struct```來存取同樣是字串的紅藍方跟目標名稱，但問題來了: `JAVA`沒有```Struct```，所以我們建造一個名為```Student```的類別來解決這個問題。

package frc.robot;

import java.util.HashMap;

public class Student {
    private HashMap<String, String> data;

    public Student(String tagAlliance, String tagName) {
        data = new HashMap<>();
        data.put("TagAlliance", tagAlliance);
        data.put("TagName", tagName);
    }

    // Getter for TagAlliance
    public String getTagAlliance() {
        return data.get("TagAlliance");
    }

    // Setter for TagAlliance
    public void setTagAlliance(String tagAlliance) {
        data.put("TagAlliance", tagAlliance);
    }

    // Getter for TagName
    public String getTagName() {
        return data.get("TagName");
    }

    // Setter for TagName
    public void setTagName(String tagName) {
        data.put("TagName", tagName);
    }
}

有了這個`Student`我們再使用集合物件優化程式碼就可以用一個新`Function`
如下:

 public Student getTag() {
        if (getTagID() <= 0  || getTagID() > 10) {//避免NullPointerException
            return null;
        }    
        HashMap<Integer, Student> tagMap = new HashMap<>();
        tagMap.put(3, new Student("Red", "Speaker"));
        tagMap.put(4, new Student("Red", "Speaker"));
        tagMap.put(5, new Student("Red", "Amp"));
        tagMap.put(9, new Student("Red", "Source"));
        tagMap.put(10, new Student("Red", "Source"));
        tagMap.put(1, new Student("Blue", "Source"));
        tagMap.put(2, new Student("Blue", "Source"));
        tagMap.put(6, new Student("Blue", "Amp"));
        tagMap.put(7, new Student("Blue", "Speaker"));
        tagMap.put(8, new Student("Blue", "Speaker"));
        return tagMap.getOrDefault(getTagID(), new Student("", ""));
    }   

這樣的話就可以直接在Command裡面使用了，具體如下:

package frc.robot.Apriltag;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.SwerveSubsystem;

public class LimelightCommand extends Command{
    private final SwerveSubsystem m_drive;
    private final LimelightSubsystem limelight;
    private final double Target = 0.8;
    private final double Zkp = 0.65;
    private final double Xkp = 0.3;
    private final double Yawkp = 0.5;
    private final String Name;
    public LimelightCommand(SwerveSubsystem m_drive,LimelightSubsystem limelight,String Name){
        this.limelight = limelight;
        this.m_drive = m_drive;
        this.Name = limelight.getTag().getTagName();
        addRequirements(m_drive);
    }
    @Override
    public void initialize(){
        limelight.setLEDMode("On");
        limelight.setCameraMode(0);
    }
    @Override
    public void execute(){
        if (limelight != null && limelight.getTv() != 0 && DriverStation.getAlliance() != null && limelight.getTag() != null &&
            !DriverStation.getAlliance().toString().isEmpty() && !limelight.getTag().getTagAlliance().isEmpty()) {
            String alliance = DriverStation.getAlliance().toString();
            String tagAlliance = limelight.getTag().getTagAlliance();
            if (alliance.equals(tagAlliance)) {
                switch (Name) {
                    case "Speaker":
                        m_drive.drive((Target + limelight.getTargetSpaceRobotPose()[2])*Zkp, 
                                      limelight.getTargetSpaceRobotPose()[0]*Xkp,
                                      limelight.getTargetSpaceRobotPose()[5]*Yawkp,
                                      true, true);
                        SmartDashboard.putString("goal", "Speaker");  
                        break;
                    case "Amp":
                        m_drive.drive((Target + limelight.getTargetSpaceRobotPose()[2])*Zkp, 
                                      limelight.getTargetSpaceRobotPose()[0]*Xkp,
                                      limelight.getTargetSpaceRobotPose()[5]*Yawkp,
                                      true, true);
                        SmartDashboard.putString("goal", "Amp");
                        break;
                    case "Source":
                        m_drive.drive((Target + limelight.getTargetSpaceRobotPose()[2])*Zkp, 
                                      limelight.getTargetSpaceRobotPose()[0]*Xkp,
                                      limelight.getTargetSpaceRobotPose()[5]*Yawkp,
                                      true, true);
                        SmartDashboard.putString("goal", "Source");
                        break;
                    default:
                        break;
                }
            } else {
                SmartDashboard.putString("goal", "Alliance color not the teamcolor");
            }
        } else {
            SmartDashboard.putString("goal", "NoGoalNow!!!!!");
        }
     
        }
    
    @Override
    public void end(boolean interrupted){
        System.out.println("Limelightend");
    }
    @Override
    public  boolean isFinished(){
        return false;
    }
}

但這個程式仍然有許多問題，像是使用Switch本身會讓程式結構變得攏長，不方便維護，跟擴展性極低......，雖然我們不能使用多型來編寫這個Limelight程式，但我們可以運用其他方法來解決這些問題:

首先我們先把我們的getTag```Function```變成這種形式，這樣才能最大化運用集合物件的概念，我們的目的是希望Limelight在看到Tag時能夠知道它的屬性，顯然上一版忘記了這個設計初衷，所以我們讓Limelight需要看到的ID都加上它的屬性，並讓Limelight在看到TagID的時候可以直接回傳它的屬性，而我們也在此加上了保護，為了避免在我們看到沒有編寫屬性的ID時系統會跳NullPointerException我們加了getOrDefault讓它直接回傳空的屬性

    public Student getTag() {
        HashMap<Integer, Student> tagMap = new HashMap<>();
        tagMap.put(3, new Student("red", "Speaker"));
        tagMap.put(4, new Student("red", "Speaker"));
        tagMap.put(5, new Student("red", "Amp"));
        tagMap.put(9, new Student("red", "Source"));
        tagMap.put(10, new Student("red", "Source"));
        tagMap.put(1, new Student("blue", "Source"));
        tagMap.put(2, new Student("blue", "Source"));
        tagMap.put(6, new Student("blue", "Amp"));
        tagMap.put(7, new Student("blue", "Speaker"));
        tagMap.put(8, new Student("blue", "Speaker"));
        tagMap.getOrDefault((int) getTagID(), new Student("", ""));// 避免NullPointerException
        return tagMap.get((int) getTagID());

    }

之後我們在student這個類別裡加上一個```Function```來接收Limelight看到的Tag的屬性

 public String[] CorrespondKey() {
        return new String[] { Fiedcharacteristic.get("TagAlliance"), Fiedcharacteristic.get("TagName") };
    }

然後我們就可以把我們的判斷式縮減成這樣一行，就能夠成功解決之前的問題

    String[] TagField = limelight.getTag().CorrespondKey();
        String Nowalliance = DriverStation.getAlliance().toString();
        if (TagField == new String[] { Nowalliance, Name }) {
            m_drive.drive((Target + limelight.getTargetSpaceRobotPose()[2]) * Zkp,
                    limelight.getTargetSpaceRobotPose()[0] * Xkp,
                    limelight.getTargetSpaceRobotPose()[5] * Yawkp,
                    true, true);
        }