---
# System prepended metadata

title: 0.5代機程式碼

---

# 0.5代機程式碼
```java=
package frc.robot;


import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;

public class Robot extends TimedRobot {

 /***********************************底盤馬達 X 4**************************************/
  WPI_TalonFX mLeftFront = new WPI_TalonFX(1);
  WPI_TalonFX mLeftBack = new WPI_TalonFX(4);
  WPI_TalonFX mRightFront = new WPI_TalonFX(2);
  WPI_TalonFX mRightBack = new WPI_TalonFX(3);

  SpeedControllerGroup leftDrive = new SpeedControllerGroup(mLeftFront, mLeftBack);
  SpeedControllerGroup  rightDrive = new SpeedControllerGroup(mRightFront, mRightBack);
  DifferentialDrive differentialDrive = new DifferentialDrive(leftDrive, rightDrive);
/************************************************************************************/



/*************************************射球********************************************/
  // 射球轉仰角motor
  public static TalonSRX aimVert = new TalonSRX(8);
  // 射球轉盤motor
  public static TalonSRX aimHori = new TalonSRX(10);
  // 射球
  public static TalonFX masterShooter = new TalonFX(5);
  public static TalonFX slaveShooter = new TalonFX(6);
/************************************************************************************/

/**************************************收球*******************************************/
//吸球
public static TalonSRX mCollector = new TalonSRX(12);
//轉盤
public static TalonSRX mTransfer= new TalonSRX(13);
//輸送
public static TalonSRX mConveyer= new TalonSRX(7);
/************************************************************************************/

/***************************************抬升***************************************** */
// 抬升水平
public static TalonSRX mLiftHori = new TalonSRX(-1);
// 抬升垂直
public static TalonSRX mLiftVertMaster = new TalonSRX(11);       
public static TalonSRX mLiftVertSlave = new TalonSRX(9);                                     
/************************************************************************************ */

/***************************************氣壓**************************************** */
//空壓機
Compressor compressor = new Compressor(1);
// 電磁閥
Solenoid sLiftUp =new Solenoid(4);
Solenoid sLIftDown =new Solenoid(5);
Solenoid sSpeedUp =new Solenoid(0);
Solenoid sSpeedDown = new Solenoid(1);
Solenoid sBallUp =new Solenoid(2);
Solenoid sBallDown = new Solenoid(3);

/********************************************************************************** */


/***********************************LimeLight************************************** */
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
    NetworkTableEntry tx = table.getEntry("tx");
    NetworkTableEntry ty = table.getEntry("ty");
    NetworkTableEntry ta = table.getEntry("ta");
/********************************************************************************** */

/******************************************搖桿 IO ***********************************/
  Joystick js1 =new Joystick(0);
  Joystick js2 =new Joystick(1);
  public final int leftStick_X = 0;
  public final int leftStick_Y = 1;
  public final int rightStick_X = 4;
  public final int rightStick_Y = 5;
  public final int trigger_L =2;
  public final int trigger_R =3;
  public final int Btn_A = 1;
  public final int Btn_B = 2;
  public final int Btn_X = 3;
  public final int Btn_Y = 4;
  public final int Btn_LB = 5;
  public final int Btn_RB = 6;
  public final int Btn_LS = 9;  
  public final int Btn_RS = 10;

/*************************************************************************************/



/****************************************狀態***************************************/
public boolean collectorContinous = false ;
public boolean gearboxContinous = false ;
public boolean liftContinous = false ;
public boolean auto_aim =false;
/******************************************************************************** */
int kSlotIdx=0;
int kTimeoutMs=30;

  @Override
  public void robotInit() {

  /***********************************馬達反轉****************************************/
    //底盤
    mLeftFront.setInverted(true);
    mLeftBack.setInverted(false);
    mRightFront.setInverted(false);
    mRightBack.setInverted(false);
    //射球
    aimHori.setInverted(false);
    aimVert.setInverted(false);
    masterShooter.setInverted(false);
    slaveShooter.setInverted(true);
    //抬升
    mLiftVertMaster.setInverted(true);
    mLiftVertSlave.setInverted(true);
    //吸球
    mCollector.setInverted(true);
    mTransfer.setInverted(false);
    mConveyer.setInverted(false);
  /**********************************************************************************/

  //跟隨
  slaveShooter.follow(masterShooter);
  mLiftVertSlave.follow(mLiftVertMaster);

  /****************************************氣壓狀態***************************************/
  collectorContinous = false ;
  gearboxContinous = false ;
  liftContinous =false;
  auto_aim =true;
  /******************************************************************************** */

  //幫補打開
  compressor.start();
  
  
  //搖桿矯正
  differentialDrive.setDeadband(0.05);


  //轉盤射球矯正
  aimHori.configForwardSoftLimitThreshold(4200);
  aimHori.configReverseSoftLimitThreshold(-4200);

  //鎖碼達
  aimHori.configFactoryDefault();
  aimHori.selectProfileSlot(kSlotIdx, 0);
  aimHori.config_kF(kSlotIdx, 0, kTimeoutMs);
  aimHori.config_kP(kSlotIdx, 0.33, kTimeoutMs);  
  aimHori.config_kI(kSlotIdx, 0.0025, kTimeoutMs);
  aimHori.config_kD(kSlotIdx, 50, kTimeoutMs);
  aimHori.config_IntegralZone(0, 400, kTimeoutMs);
  //最小
  aimHori.configNominalOutputForward(0, kTimeoutMs);
  aimHori.configNominalOutputReverse(0, kTimeoutMs);
  //最大
  aimHori.configPeakOutputForward(0.17, kTimeoutMs);
  aimHori.configPeakOutputReverse(-0.17, kTimeoutMs);

  //Encorder 反轉
  aimVert.setSensorPhase(true);
   }

  
  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
  }

  
  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {
    
    // 影像辨識
    double x = tx.getDouble(0.0);
    double y = ty.getDouble(0.0);
    double area = ta.getDouble(0.0);
    double  forDis=-0.02*Math.pow(y, 3)-0.25*Math.pow(y, 2)-9.39*y+173.87;

    SmartDashboard.putNumber("LimelightX", x);
    SmartDashboard.putNumber("LimelightY", y);
    SmartDashboard.putNumber("LimelightArea", area);
    SmartDashboard.putNumber("masterShooter_Velocity", masterShooter.getSelectedSensorVelocity());
    SmartDashboard.putNumber("masterShooter_Temperature", slaveShooter.getTemperature());
    SmartDashboard.putNumber("masterShooter_percent", masterShooter.getMotorOutputPercent());
    SmartDashboard.putNumber("slaveShooter_percent", slaveShooter.getMotorOutputPercent());
    SmartDashboard.putNumber("Distance", forDis);
 /*   SmartDashboard.putNumber("aimHori_Position", aimHori.getSelectedSensorPosition());
    SmartDashboard.putNumber("aimVert_Position", aimVert.getSelectedSensorPosition());
    SmartDashboard.putNumber("1", aimVert.getSelectedSensorPosition());*/




    /************************  駕駛一  ********************** */
    //RB:按:底盤功率1 底盤功率0.5
    double chassisSpeed = 0.5;
    if(js1.getRawButton(Btn_RB))
      chassisSpeed = 1;
    //底盤控制
    differentialDrive.arcadeDrive(js1.getRawAxis(leftStick_Y)*chassisSpeed,js1.getRawAxis(rightStick_X)*chassisSpeed);
   
 
    //LB:氣壓變速
    if(js1.getRawButtonPressed(Btn_LB))
    {
      gearboxContinous= !gearboxContinous;
      sSpeedUp.set(gearboxContinous);
      sSpeedDown.set(!gearboxContinous);
      Timer.delay(0.1);

    }

    //LS_Btn:抬升氣壓
    if(js1.getRawButtonPressed(Btn_LS))
    {
      liftContinous= !liftContinous;
      sLiftUp.set(liftContinous);
      sLIftDown.set(!liftContinous);
      Timer.delay(0.1);
    }
   


    //Y:抬升上拉
    if(js1.getRawButton(Btn_Y))
    {
      mLiftVertMaster.set(ControlMode.PercentOutput,-1);
      mLiftVertSlave.set(ControlMode.PercentOutput, -1);
    }//A:抬升下移
    else if(js1.getRawButton(Btn_A))
    {
      mLiftVertMaster.set(ControlMode.PercentOutput,1);
      mLiftVertSlave.set(ControlMode.PercentOutput, 1);
    }
    else
    {
      mLiftVertMaster.set(ControlMode.PercentOutput,0);
      mLiftVertSlave.set(ControlMode.PercentOutput, 0);
    }

    /************************************************************** */

    

    /***************************駕駛二*************************** */
    // X 氣壓收球伸縮 收和一顆
    if(js2.getRawButtonPressed(Btn_X))
    {
      collectorContinous= !collectorContinous;
      sBallUp.set(collectorContinous);
      sBallDown.set(!collectorContinous);
      Timer.delay(0.1);
    }

    // Trigger_L:吸球+輸送盤旋轉
    if(js2.getRawAxis(trigger_L)>=0.2)
       {
      mCollector.set(ControlMode.PercentOutput,js2.getRawAxis(3)/1.5);
      mTransfer.set(ControlMode.PercentOutput, 0.5);
    }
    else{
      mTransfer.set(ControlMode.PercentOutput, 0);
      mCollector.set(ControlMode.PercentOutput,0);
    }

    //B 射球+輸送盤旋轉+傳送帶
    if(js2.getRawButton(Btn_B))
    {
      mTransfer.set(ControlMode.PercentOutput, 0.35);
      mConveyer.set(ControlMode.PercentOutput, 0.5);
    }
    else{
      mTransfer.set(ControlMode.PercentOutput, 0);
      mConveyer.set(ControlMode.PercentOutput, 0);
    }

    if(js2.getRawButtonPressed(Btn_LB))
    {
      auto_aim =! auto_aim;
      Timer.delay(0.1);
    }

    //敞篷控制
    
    if(js2.getRawButton(Btn_A))
    {
      aimHori.setSelectedSensorPosition(0);
    }

    if(auto_aim)
    {
    //砲台自動對正
    int turret =30; //砲台旋轉對影像的轉換
    int nw_pos=aimHori.getSelectedSensorPosition();
    //砲台調整
    if(area>0.00005) // 目標區域太小
      aimHori.set(ControlMode.Position,nw_pos+x*turret);
    else 
      aimHori.set(ControlMode.Position, 0);
    }
    else{
      //LS_Y: 手動砲台控制
      aimHori.set(ControlMode.PercentOutput, js2.getRawAxis(rightStick_X)*0.2);
      //RS_X: 手動敞篷控制 
      aimVert.set(ControlMode.PercentOutput, js2.getRawAxis(leftStick_Y)*0.1);
    }
    /******************************************************************** */
    

  }



  @Override
  public void testInit() {
  }
  
  @Override
  public void testPeriodic() {




    double x = tx.getDouble(0.0);
    double y = ty.getDouble(0.0);
    double area = ta.getDouble(0.0);
    double  forDis=-0.02*Math.pow(y, 3)-0.25*Math.pow(y, 2)-9.39*y+173.87;
    SmartDashboard.putNumber("Distance", forDis);
    SmartDashboard.putNumber("Temperature",masterShooter.getTemperature());
    SmartDashboard.putNumber("Velocity",masterShooter.getSelectedSensorVelocity());
    SmartDashboard.putNumber("hat_position",aimVert.getSelectedSensorPosition());
    SmartDashboard.putNumber("turret_position",aimHori.getSelectedSensorPosition());
    int turret =30; //砲台旋轉對影像的轉換
    int nw_pos=aimHori.getSelectedSensorPosition();
    aimHori.set(ControlMode.Position,nw_pos+x*turret);
    

    //B 射球+輸送盤旋轉+傳送帶
    if(js2.getRawButton(Btn_B))
    {
      mTransfer.set(ControlMode.PercentOutput, 0.35);
      mConveyer.set(ControlMode.PercentOutput, 0.5);
    }
    else{
      mTransfer.set(ControlMode.PercentOutput, 0);
      mConveyer.set(ControlMode.PercentOutput, 0);
    }



   if(js2.getRawButton(Btn_A))
      aimHori.setSelectedSensorPosition(0);
    if(js2.getRawButton(Btn_X))
      aimVert.setSelectedSensorPosition(0);

    aimVert.set(ControlMode.PercentOutput, js2.getRawAxis(leftStick_Y)*0.2);

  }
}
```