# 20211115~20211121 程式組
###### tags: `工作筆記 程式組`

## 20211116
### 黃冠穎
網路找到auto aim的範本改的，但看起來似乎只有瞄準沒有射球，我下禮拜二會再試試看，看要不要加射球馬達的。

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.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;

public class Robot extends TimedRobot {
  private final Joystick m_joystick = new Joystick(0);

  WPI_TalonFX m_left_master_talon = new WPI_TalonFX(1);
  WPI_TalonFX m_right_master_talon = new WPI_TalonFX(2);
  WPI_TalonFX m_left_slave_talon = new WPI_TalonFX(3);
  WPI_TalonFX m_right_slave_talon = new WPI_TalonFX(4);
  /*WPI_TalonFX shoter1 = new WPI_TalonFX(5);
  WPI_TalonFX shoter2 = new WPI_TalonFX(6);
  WPI_TalonSRX trans1 = new WPI_TalonSRX(8);
  WPI_TalonSRX trans2 = new WPI_TalonSRX(10);*/
  SpeedControllerGroup LeftDrive = new SpeedControllerGroup(m_left_master_talon,m_left_slave_talon);
  SpeedControllerGroup RightDrive = new SpeedControllerGroup(m_right_master_talon,m_right_slave_talon);

  DifferentialDrive Drive = new DifferentialDrive(LeftDrive,RightDrive);

  Joystick js1 = new Joystick(0);

  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;

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

  private final double max_auto_output = 0.5;

  private final double kP_dist = 0.0; // First tune the turn PD loop
  private final double dist_tol = 0.5;
  private final double desired_vert_angle = 10;

  private final double kP_turn = 0.025;
  private final double kD_turn = kP_turn * 10;
  private final double angle_tol = 0.5;
  private final double max_anglular_vel = 15; // Deg/Sec
  private final double max_anglular_accel = 10; // Deg/Sec^2

  private final ProfiledPIDController m_controller = new ProfiledPIDController(kP_turn, 0.0, kD_turn,
      new TrapezoidProfile.Constraints(max_anglular_vel, max_anglular_accel));

  @Override
  public void robotInit() { // This is just boilerplate code to setup the talons

    // Ensures we don't have any previously set configs
    m_left_master_talon.configFactoryDefault();
    m_right_master_talon.configFactoryDefault();
    m_left_slave_talon.configFactoryDefault();
    m_right_slave_talon.configFactoryDefault();

    // Make sure we are inverted correctly so we don't break things
    m_left_master_talon.setInverted(true);
    m_right_master_talon.setInverted(true);
    m_left_slave_talon.setInverted(true);
    m_right_slave_talon.setInverted(true);


    // Make the slaves follow the masters so they're in sync
    m_left_slave_talon.follow(m_left_master_talon);
    m_right_slave_talon.follow(m_right_master_talon);
  }

  @Override
  public void robotPeriodic() {
    Drive.tankDrive(js1.getRawAxis(1)*0.8,js1.getRawAxis(4)*0.8);
  }

  @Override
  public void autonomousInit() {
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {
    // Get the horizantal and vertical angle we are offset by
    double horizantal_angle = tx.getDouble(0.0);
    double vertical_angle = ty.getDouble(0.0);

    // Goal angle - current angle
    double distance_error = desired_vert_angle - vertical_angle;
    double forward_output = 0;
    double turn_output = 0;

    // Have a deadband where we are close enough
    if (Math.abs(horizantal_angle) > angle_tol) {
      // Get PID controller output
      turn_output += m_controller.calculate(horizantal_angle);
    }

    // Have a deadband where we are close enough
    if (Math.abs(distance_error) > dist_tol) {
      // Just a proportional gain here
      forward_output += kP_dist * distance_error;
    }

    if (js1.getRawButton(1)) { // If button is held, auto aim
      // Forward output is the same on both sides but turn is opposite
      m_left_master_talon.set(ControlMode.PercentOutput, deadband(forward_output + turn_output));
      m_right_master_talon.set(ControlMode.PercentOutput, deadband(forward_output - turn_output));
    } else { // Stop Moving
      m_left_master_talon.set(ControlMode.PercentOutput, 0);
      m_right_master_talon.set(ControlMode.PercentOutput, 0);
    }

    if(js1.getRawAxis(1)>0 || js1.getRawAxis(1)<0){
      m_left_master_talon.set(ControlMode.PercentOutput,js1.getRawAxis(1)*0.8);
      m_right_master_talon.set(ControlMode.PercentOutput,js1.getRawAxis(1)*0.8);
    }
    else if(js1.getRawAxis(4)>0 || js1.getRawAxis(4)<0){
      m_left_master_talon.set(ControlMode.PercentOutput,js1.getRawAxis(4)*-0.7);
      m_right_master_talon.set(ControlMode.PercentOutput,js1.getRawAxis(4)*0.7);
    }else{
      m_left_master_talon.set(ControlMode.PercentOutput,0);
      m_right_master_talon.set(ControlMode.PercentOutput,0);
    }

  }


public double deadband(double a) {
    return Math.abs(a) < max_auto_output ? a : Math.copySign(max_auto_output, a);
  }

  @Override
  public void disabledInit() {}

  
  @Override
  public void disabledPeriodic() {}

  
  @Override
  public void testInit() {}

  
  @Override
  public void testPeriodic() {}
}


}

### 林俊彥
今天有試著上機測試 我預想的讓機器人跟著走是用勾股定理 還有點距還算要跑多少距離 而且JAVA的算數運算指令好不一樣 但我發現這樣好像 可能 不能考慮到轉彎的角度 我猜可能結合PID可以做出來 所以下一節課我要嘗試結合兩個東西

### 吳玠廷
今天研究官方給的limelight程式碼因為式C++的語法所以要把它換成java的，但宣告一直找不到在哪裏大致上把所有官方程式碼看了一遍。

## 20211118

### 黃冠穎
今天幫黃紫紅慶生大概7:15才回到地下室，接著又去試了隊服，所以試完大概已經7:35,7:40了吧。剩下的時間也不夠看一部CSS的影片，所以我打算改一下禮拜二的程式碼，預計是讓機器人能用tank drive加上按A能自動瞄準，但機器人一直顯示連線問題，可能段考後再試試看。若順利，希望能再加上射球的程式碼完成真正的auto aim。

### 吳玠廷
今天也跟平常一樣剝線然偶後接線外加量隊服，後來的時間我們在焊接。