20220103~20220109 程式組

package frc.robot;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Units;

public class Robot extends TimedRobot {
  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  // PWM channels
  public final int kFrontLeftChannel = 3;
  public final int kRearLeftChannel = 4;
  public final int kFrontRightChannel = 1;
  public final int kRearRightChannel = 2;
  public final int joystickPort = 0;

  // driver station ports
  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;

  // Motors
  WPI_TalonFX m_frontLeft = new WPI_TalonFX(kFrontLeftChannel);
  WPI_TalonFX m_rearLeft = new WPI_TalonFX(kRearLeftChannel);
  WPI_TalonFX m_frontRight = new WPI_TalonFX(kFrontRightChannel);
  WPI_TalonFX m_rearRight = new WPI_TalonFX(kRearRightChannel);

  // SpeedControllerGroup and DifferentialDrive
  SpeedControllerGroup leftGroup = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
  SpeedControllerGroup rightGroup = new SpeedControllerGroup(m_frontRight, m_rearRight);
  DifferentialDrive drive = new DifferentialDrive(leftGroup, rightGroup);

  public MecanumDrive m_Drive = new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);

  Encoder encoder = new Encoder(0, 1);

  // Joystick
  Joystick joystick = new Joystick(joystickPort);

  @Override
  public void robotInit() {
    // motors inverted
    m_frontRight.setInverted(true);
    m_rearRight.setInverted(true);
    m_frontLeft.setInverted(false);
    m_rearLeft.setInverted(false); 
  }

  @Override
  public void robotPeriodic() {
    SmartDashboard.putNumber("key", m_frontLeft.getSelectedSensorVelocity());
  }

  @Override
  public void autonomousInit() {}

  /** This function is called periodically during autonomous. */
  @Override
  public void autonomousPeriodic() {}

  /** This function is called once when teleop is enabled. */
  @Override
  public void teleopInit() {}

  /** This function is called periodically during operator control. */
  @Override
  public void teleopPeriodic() {
    // drivetrain
    double ySpeed = -joystick.getRawAxis(leftStick_Y) * 0.5;
    double xSpeed = joystick.getRawAxis(leftStick_X) * 0.5;
    double zRotation = joystick.getRawAxis(rightStick_X) * 0.5;

    m_frontLeft.set(ySpeed + xSpeed + zRotation);
    m_frontRight.set(ySpeed - xSpeed - zRotation);
    m_rearLeft.set(ySpeed - xSpeed + zRotation);
    m_rearRight.set(ySpeed + xSpeed - zRotation);

    
    
  }

  /** This function is called once when the robot is disabled. */
  @Override
  public void disabledInit() {}

  /** This function is called periodically when disabled. */
  @Override
  public void disabledPeriodic() {}

  /** This function is called once when test mode is enabled. */
  @Override
  public void testInit() {}

  /** This function is called periodically during test mode. */
  @Override
  public void testPeriodic() {}
}