owned this note
owned this note
Published
Linked with GitHub
# 昱翔 - 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);
}
}
```
###### tags: `程式組教程`
Google Drive
Gist
Clipboard
Sign in with Wallet
