# 20211206~20211212 程式組
###### tags: `工作筆記 程式組`

## 20211207

### 陳伯豪

這個禮拜二我有先完成了 MecanumDrive 的兩個寫法，一個是自己手動寫的方法，然後另一個是直接利用 FRC 官方給的函式庫下去寫，我把兩個都丟在下面

然後我預計這禮拜四去讀和寫一些超音波

:::spoiler 自己寫的

package frc.robot;

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

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;
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;

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

  public final int kFrontLeftChannel = 3;
  public final int kRearLeftChannel = 4;
  public final int kFrontRightChannel = 1;
  public final int kRearRightChannel = 2;

  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;

  double ySpeed;
  double xSpeed;
  double zRotation;

  WPI_TalonSRX LeftFront = new WPI_TalonSRX(kFrontLeftChannel);
  WPI_TalonSRX LeftRear = new WPI_TalonSRX(kRearLeftChannel);
  WPI_TalonSRX RightFront = new WPI_TalonSRX(kFrontRightChannel);
  WPI_TalonSRX RightRear = new WPI_TalonSRX(kRearRightChannel);

  Joystick joystick = new Joystick(0);

  @Override
  public void robotInit() {
    RightFront.setInverted(true);
    RightRear.setInverted(true);
    LeftFront.setInverted(false);
    LeftRear.setInverted(false);
  }

  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {

    ySpeed = -joystick.getRawAxis(leftStick_Y) * -0.5;
    xSpeed = joystick.getRawAxis(leftStick_X)* -0.5;
    zRotation = joystick.getRawAxis(rightStick_X)* 0.5;

    LeftFront.set(ySpeed + xSpeed + zRotation);
    RightFront.set(ySpeed - xSpeed - zRotation);
    LeftRear.set(ySpeed - xSpeed + zRotation);
    RightRear.set(ySpeed + xSpeed - zRotation);
  }

  @Override
  public void disabledInit() {
  }

  @Override
  public void disabledPeriodic() {
  }

  @Override
  public void testInit() {
  }

  @Override
  public void testPeriodic() {
  }
}

:::

:::spoiler 使用官方函式庫

package frc.robot;

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

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

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

  public final int kFrontLeftChannel = 3;
  public final int kRearLeftChannel = 4;
  public final int kFrontRightChannel = 1;
  public final int kRearRightChannel = 2;

  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 MecanumDrive m_Drive;

  double ySpeed;
  double xSpeed;
  double zRotation;

  double chassisPower;

  WPI_TalonSRX m_frontLeft = new WPI_TalonSRX(kFrontLeftChannel);
  WPI_TalonSRX m_rearLeft = new WPI_TalonSRX(kRearLeftChannel);
  WPI_TalonSRX m_frontRight = new WPI_TalonSRX(kFrontRightChannel);
  WPI_TalonSRX m_rearRight = new WPI_TalonSRX(kRearRightChannel);

  Joystick joystick = new Joystick(0);

  WPI_TalonSRX LeftFront = new WPI_TalonSRX(0);
  WPI_TalonSRX LeftRear = new WPI_TalonSRX(1);
  SpeedControllerGroup LeftMotor = new SpeedControllerGroup(LeftFront, LeftRear);

  WPI_TalonSRX RightFront = new WPI_TalonSRX(2);
  WPI_TalonSRX RightRear = new WPI_TalonSRX(3);
  SpeedControllerGroup RightMotor = new SpeedControllerGroup(RightFront, RightRear);

  @Override
  public void robotInit() {
    double chassisPower = 0.5;
    m_Drive = new MecanumDrive(LeftFront, LeftRear, RightFront, RightRear);
  }

  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {

    if (joystick.getRawButton(6) || joystick.getRawButton(5)){
      chassisPower = 1;
    } else {
      chassisPower = 0.5;
    }

    ySpeed = -joystick.getRawAxis(leftStick_Y) * chassisPower;
    xSpeed = joystick.getRawAxis(leftStick_X) * chassisPower;
    zRotation = joystick.getRawAxis(rightStick_X) * chassisPower;

    m_Drive.driveCartesian(ySpeed, -xSpeed, zRotation);
  }

  @Override
  public void disabledInit() {
  }

  @Override
  public void disabledPeriodic() {
  }

  @Override
  public void testInit() {
  }

  @Override
  public void testPeriodic() {
  }
}

:::

### 魏仁祥

今天開完會後，我去忙大機器人的設定。簡單來說，大機器人的 radio 有問題，我在 configure 時會失敗。我有問南科的指導老師了，禮拜四在試試看，但說真的我覺得很浪費時間，有可能會丟給學弟做。

### 黃冠穎
今天看了PathWeaver官網的資料，要隨便畫出一個path和path group，但其中的程式今天只大略的看了一下，禮拜四會再繼續看。

今天limelight把測距寫完了病多寫了一點讓他移動的程式，但不知道為麼我只要寫到"tx" "ty" "tz"之類的就會出現error。

## 20211209
### 黃冠穎
今天接續禮拜二的PathWeaver官網資料，但看著看著，發現要看的變多了，從本來的pathweaver introduction，到變成還要看frc charaterization, introduction to trafectory,feedforward control和複習pid control。總之呢，我會慢慢看完，理解不理解到時再說。