---
# System prepended metadata

title: 這是配電好的程式--MecanumDrive

---

# 這是配電好的程式--MecanumDrive
Js1:
mecanumDrive
X:抬升左移
Y:抬升上拉
A:抬升下移
B:抬升右移
LS_Btn:氣壓變速
LB:抬升氣壓
RB:按:底盤功率1 不按:底盤功率0.5

Js2:
X:氣壓收球 收球合一
Trigger_L:吸球+旋轉
B:射球+旋轉
LB:射球轉換 自動/手動 模式
LS_Y:敞篷調整
RS_X:砲台調整

 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;


---


### Talon number

![](https://i.imgur.com/8DfZ8Fi.jpg)
上微動:0
下微動:1

|  talon   |   1  |   2  |  3   |   4  |  5   |   6  |   7  |  8   |  9   |    10 | 11   | 12 | 13|
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | -------- | -------- | -------- |
|   轉向  |   +  | +    | -    | -    |     |     |  +   |   +  |     |  10   | -     | +(up)    | -     |
## 收球
```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(2);

 WPI_TalonFX mRightFront = new WPI_TalonFX(4);

 WPI_TalonFX mRightBack = new WPI_TalonFX(3);

  
  
  
  
  

/*************************************射球********************************************/

 // 出球

 public static TalonSRX ballout=new TalonSRX(8);

 // 射球轉盤motor

 public static TalonSRX aimHori = new TalonSRX(9);

 // 射球

 public static TalonFX masterShooter = new TalonFX(5);

 public static TalonFX slaveShooter = new TalonFX(6);

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

  

/**************************************收球*******************************************/

//吸球

public static TalonSRX mCollector = new TalonSRX(13);

//轉盤

public static TalonSRX mTransfer = new TalonSRX(11);

public static TalonSRX mTransfer2 = new TalonSRX(10);

//輸送

public static TalonSRX mConveyer = new TalonSRX(7);

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

  

/***************************************抬升***************************************** */

// 抬升水平

//public static TalonSRX mLiftHori = new TalonSRX(0);

// 抬升垂直

//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(false);

 mLeftBack.setInverted(false);

 mRightFront.setInverted(true);

 mRightBack.setInverted(true);

 //射球

 aimHori.setInverted(false);

 //aimVert.setInverted(false);

 masterShooter.setInverted(false);

 slaveShooter.setInverted(true);

 //抬升

 //mLiftVertMaster.setInverted(true);

 //mLiftVertSlave.setInverted(true);

 //吸球

 mCollector.setInverted(true);

 mTransfer.setInverted(false);

 mTransfer2.setInverted(true);

 mConveyer.setInverted(false);

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

  

 //跟隨

 slaveShooter.follow(masterShooter);

 //mLiftVertSlave.follow(mLiftVertMaster);

  

 /****************************************氣壓狀態***************************************/

collectorContinous = false ;

gearboxContinous = false ;

liftContinous =false;

auto_aim =true;

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

  

 //幫補打開

 compressor.start();

 //搖桿矯正

 //differentialDrive.setDeadband(0.05);

}

  
  

 //轉盤射球矯正

 //.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))

    {

 //.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.getRawAxis(3)>0.2)

    {

 mCollector.set(ControlMode.PercentOutput,0.65);

 mConveyer.set(ControlMode.PercentOutput,1);

 mTransfer.set(ControlMode.PercentOutput,1);

 mTransfer2.set(ControlMode.PercentOutput,1);

    }

 else if(js2.getRawAxis(2)>0.2)

    {

 mCollector.set(ControlMode.PercentOutput,-0.65);

 mConveyer.set(ControlMode.PercentOutput,-1);

 mTransfer.set(ControlMode.PercentOutput,-1);

 mTransfer2.set(ControlMode.PercentOutput,-1);

    }

 else{

 mCollector.set(ControlMode.PercentOutput,0);

 mConveyer.set(ControlMode.PercentOutput,0);

 mTransfer.set(ControlMode.PercentOutput,0);

 mTransfer2.set(ControlMode.PercentOutput,0);

    }

if(js2.getRawButton(Btn_Y)){

 ballout.set(ControlMode.PercentOutput,1);

}

else if(js2.getRawButton(Btn_A)){

 ballout.set(ControlMode.PercentOutput,-1);

}

else{

 ballout.set(ControlMode.PercentOutput,0);

}

 // 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);\*/

  

  }

}


```

