# 20210705~20210711 程式組 暑期計畫
###### tags: `工作筆記 程式組`

### 魏仁祥
今天是從介紹積分( integral )開始，在第一次的試驗當中(ki = 0.5)，因為一開始距離 setpoint (10 feet) 較遠，也因此 error 較大，而加在一起時 errorsum 就會超大所以馬達輸出的馬力也超大，導致機器人衝過頭。


接著在嘗試後，它設定了 integral limit，也就是只有距離在 setpoint 固定的範圍內，它才會開始計算 Integral Output。而在設定這個距離時也要注意，機器人若是無法達到此範圍內而開始停住的話，就沒有積分( integral )協助它慢慢地抵達 setpoint，而當然這個 limit 的數字越小越好。

$$ Integral Output = kI × errorSum $$

$$ Derivative Output = KD × errorRate $$

$$ Motor Output = kp × error + kI × errorSum + KD × errorRate$$

$$ Output(t) = kp × e(t) +ki × \int_{0}^{t}e(t)dt + kd × \dfrac{d}{dt} e(t)$$

在最後是介紹 Derivative term，好像就是試試而已我覺得沒什麼...
我好像覺得這個比較沒什麼用...，感覺就只是輔助用而已，就類似預測下一個瞬間醬


:::spoiler my code

package frc.robot;

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

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


public class Robot extends TimedRobot {

  private WPI_TalonFX leftMotor1 = new WPI_TalonFX(1);
  private WPI_TalonFX leftMotor2 = new WPI_TalonFX(2);
  private WPI_TalonFX rightMotor1 = new WPI_TalonFX(3);
  private WPI_TalonFX rightMotor2 = new WPI_TalonFX(4);

  private Joystick js1 = new Joystick(0);


  @Override
  public void robotInit() {
  }

  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
      encorder.reset();
      setpoint = 0;
      lastError = 0;
      lasteTimestamp = Timer.getFPGATimestamp(); 
  }

  final double kP = 0.5;
  final double kI = 0.5
  final double kD = 0.1;
  final double iLimit = 1;
  
  double setpoint = 0;
  double errorSum = 0;
  double lasteTimestamp = 0;
  double lastError = 0;
  
  @Override
  public void autonomousPeriodic() {
      if(js1.getRawButton(1)) setpoint = 10;
      else if(js1.getRawButton(2)) setpoint = 0;
      
      // get sensor position
      double sensorPosition = encoder.get() * kDriveTick2Feet;
  
      // calcculation
      double error = setpoint - sensorPosition;
      double dt = Timer.getFPGATimestamp() - lasteTimestamp;
      
      if(Math.abs(error)<iLimit) errorSum += error * dt;
      
      double errorRate = (error - lastError) / dt;
      
      double outputSpees = kP * error + kI * errorSum + kD * errorRate;
      
      // output to motors
      leftMotor1.set(outputSpeed);
      leftMotor2.set(outputSpeed);
      rightMotor1.set(-outputSpeed);
      rightMotor2.set(-outputSpeed);
      
      // update last var
      lasteTimestamp = Timer.getFPGATimestamp();
      lastError = error;
  }
  
  
  
  @Override
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {
  }

  @Override
  public void disabledInit() {
  }

  @Override
  public void disabledPeriodic() {
  }

  @Override
  public void testInit() {
  }

  @Override
  public void testPeriodic() {
  }
}

:::

### 陳伯豪

這次的影片主要是在介紹PID系統裡面的Integral term（積分項）和Derivative term（導數項）
因為這次的影片算是稍微將PID系統做離一個段落，因此我就順便都打出來吧(●'◡'●)

1.Proprotional Term
。前提：
　－希望利用Proprotional term（比例項）來讓機器人停留在10 M（可更改）的位置
。公式：
　－Output（電機輸出） = Kp（常數） * Error（目前位置距離目標位置的差值）
。解釋：
　－將一常數去乘上一個會隨著移動更改的變數項，在藉由此結果來產生越接近設定位置速度會越來越低的效果
。問題：
　－單單只使用Proprotional term（比例項）無法精準的停留在10 M這個位置，影片中測試了幾次後找出最適合那台機器的Kp值是0.5（不適用於其他機器人），要是將0.5套用在上面的公式中會出現非常接近10 M但還是無即時停止並修正的狀況（見下圖）

⇂ Proprotional term最佳效果圖

2.Integral term
。前提：
　－由於單單只使用Proprotional Term無法達到精準停留在10 M（可更改）上，因此想加上Integral term（積分項）來針對無法準確停留在10 M做處理
。公式：
　－Output = Kp * Error + Ki（常數） * erroeSum（誤差值過去一段時間的和，也就是誤差和）
。解釋：
　－考慮到過去的誤差，將(誤差和)乘以一個的常數。一個簡單的比例系統會前後調整，因此會在預定值的附近來回變化。通過加上負的誤差值，系統誤差值就會漸漸減少。所以，最終這個PID迴路系統會在應該會在預設值穩定下來。
。問題
　－藉由Proprotional Term和Integral term的共同作用，此機器人已經得到精準停在10 M上的能力(當Ki = 0.5時)，但是還是出現了另一個問題：在達到10 M前會有一段爆衝(？)的這個狀況（見下圖）

⇂ Proprotional Term和Integral term最佳效果圖

2-1.Integral Limit
。用途：
　－利用i-Limit值來降低爆衝的量

⇂ 加上i-Limit前後的比較圖

3.Derivative term
。前提：
　－雖然Proprotional Term和Integral term已經能夠做到精準停留在10 M上，但是因為在到達前會有一段衝過頭的狀況，所以想藉由Derivative term（導數項）來解決衝過頭這個問題
。公式：
　－Output = Kp * Error + Ki * erroeSum + Kd（常數） * ErorRate（類似預測將來的一個值？）
。解釋：
　－導數項是類似於考慮將來誤差，計算誤差的一種....東西（？）。這個導數的控制會對系統的改變作出反應。導數的結果越大，那麼控制系統就對輸出結果作出更快速的反應。
。結果
　－成功達到了精準在10 M停下來（Kd = 0.1）且不會有過衝的情形（見下圖）

⇂ PID系統最佳效果圖

### 黃冠穎
看完這周的影片，相較於上周只有kP控制馬達輸出，這周多出了kI跟kD兩個，kI是利用多一個變數errorSum去讓接近10feet的時候能更小的馬達輸出，kD則是多一個errorRate，讓機器人在接近10feet時自動縮小馬達輸出。三個kP、kI、kD一起運作起來才是真正的PID控制以後也可以看情況用，不一定每次都要用kP、kI、kD，可以用kP+kI或kP一個就好，看情況。