# 20220207~20220213 程式組寒訓

###### tags: `工作筆記 程式組`

# 20220207

## 陳伯豪

### 今日進度（程式）

**1. CommandBase**

* 已經寫的差不多了，我這一兩天應該會跟其他已經寫好的人對一下 CommandBase 的 Code

### 遇到的問題

**1. 整體進度**

* 今天因為對其他東西不夠了解所以被范政佑念:sweat_smile:

### 解決方法

**1. 整體進度**

* 之後幾天我應該不會去找其他新的東西，目前主要以把程式組其他人在用的東西學起來（LimeLight、路徑規劃、Command Base等）

### 結論

明天應該會以寫完 Command Base 跟學一些其他人在用的東西為主

## 黃冠穎

昨天晚上沒睡所以中午就走了

### 進度

* mecanum drive程式的調整

好，其實我也不是很清楚為啥之前可以用，但王志愷說輪子裝反了，裝正後我調整程式碼，明天去看看底盤有沒有問題。

* 測 Trajectory following

沒測出來，結果Aaron一下午就弄好了。

### 問題

* Trajectory following

Aaron好像解決了，明天去了解一下

## 王鴻霖 Aaron Wang

今天是我目前為止最有進度的一天

### 進度大綱

* Mecanumdrive, Mecanumdrive Odometry 及馬達操控類別（motor controller）針對測試用原型機做了適配（詳情見以下問題區域）
* 測試運用 MecanumControllerCommmand 類別進行 Trajectory Following 及調整
* 針對原型機重新做了一次 Characterization 以求更準確的測試結果

### 問題與解決過程

!!! 這裡我會記錄一些很常見的問題之後遇到可以參考看看我的解決方案 !!!

為了測試MecanumControllerCommand, 我開了一個MecanumControllerCommand example project (在create project的example project list裡)，然後針對原型機做調整，中間遇到大量問題，所幸解決了

1. 馬達適配

Example code 裡面使用的是 SparkMax 馬達，透過 PWMSparkMax Object 來控制，這個類別有Implement MotorController 和 SpeedController, 所以可以被MecanumDrive 採用。(MecanumDrive是包裝好的麥卡納姆輪專用驅動控制物件，通常要與MecanumDriveOdometry和MecanumDriveKinematics一起使用，而MecanumControllerCommand也必須使用到MecanumDrive，所以MecanumDrive的設定其實就是一切的基礎) 原型機使用的馬達是TalonSRX，所以我找了最相近的PWMTalonSRX使用。<br/>

**注意，這步非常錯誤！**<br/>

我們的馬達是透過CAN接口，所以PWM用的物件當然不管用。測試的時候，原型機自然沒有動。我一開始想說可以先調到ctre library 的TalonSRX物件來操控馬達，但那是最底層的馬達控制，並沒有implement SpeedController，就算成功控制馬達也不能拿到MecanumDrive裡面用，所以我找了一下我們隊之前github上的code，發現有一個叫做WPI_TalonSRX的物件好像可以用，就拿來試試。<br/>
BTW，WPI_TalonSRX當初被拿來用的主要原因其實是因為我發現PWMTalonSRX沒有辦法回傳TalonSRX integrated encoder的數值，所以才採用，只是剛好巧合他其實是本來整個東西就用他就好。<br/>
WPI_TalonSRX其實是Phoenix為了WPI library客製化的MotorController/SpeedController Class。(也就是包裝好的TalonSRX class) 同理，MecanumDrive自然就可以使用他。要注意的是，Phoenix系列的MotorController Constructor通常是接收DeviceID不是Port(DeviceID可以在Phoenix Tuner裡面看到)，對應的ID我就直接在Constants.java裡做了修改。最後能用的解決方案其實非常簡單：

  private final WPI_TalonSRX motorFL = new WPI_TalonSRX(DriveConstants.kFrontLeftMotorID);
  private final WPI_TalonSRX motorRL = new WPI_TalonSRX(DriveConstants.kRearLeftMotorID);
  private final WPI_TalonSRX motorFR = new WPI_TalonSRX(DriveConstants.kFrontRightMotorID);
  private final WPI_TalonSRX motorRR = new WPI_TalonSRX(DriveConstants.kRearRightMotorID);

  private final MecanumDrive m_drive =
      new MecanumDrive(motorFL, motorRL, motorFR, motorRR);

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;就這麼簡單，但害我看Documentation看超久。

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;修MotorController的過程我也修正了其他大量的小問題，這裡就不多說了。

2. Encoder數據取得
原本Example Code 是使用外接的Encoder，但TalonSRX是使用內建的。查詢之後網路上會有一堆人叫你用TalonSRX object或是 CANTalon，但那些都不適配我們的狀況<br/>
實際上解答同樣在WPI_TalonSRX object中!<br/>
WPI_TalonSRX有一系列如getSelectedSensorPosition()、getSelectedSensorVelocity()、setSelectedSensorPosition()等method，可以回傳內建sensor的數據。<br/>
`getSelectedSensorPosition()` -> 回傳已轉動角度(in radian)
`getSelectedSensorVelocity()` -> 回傳當下速率(in radian)
`setSelectedSensorPosition()` -> 設定Encoder的目前角度，主要用來reset Encoder<br/>
從這裡可以看到大部分東西回傳之後都需要做單位轉換，舉例來說：

m_odometry.update(
        m_gyro.getRotation2d(),
        new MecanumDriveWheelSpeeds(
            motorFL.getSelectedSensorVelocity()* (10.0 / 4096) * DriveConstants.kWheelCircumference,
            motorFL.getSelectedSensorVelocity()* (10.0 / 4096) * DriveConstants.kWheelCircumference,
            motorFL.getSelectedSensorVelocity()* (10.0 / 4096) * DriveConstants.kWheelCircumference,
            motorFL.getSelectedSensorVelocity()* (10.0 / 4096) * DriveConstants.kWheelCircumference));

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;在update Odometry class 的時候要轉換成公尺

3. Trajectory 的Path檔讀取不到
其實只是小問題，不過紀錄一下以免遇到類似錯誤又浪費時間
官方給的讀path檔範例差不多長這樣：

String trajectoryJSON = "paths/testPath.wpilib.json";
        try {
            Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
            Trajectory testTrajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
} catch (IOException e) {
            DriverStation.reportError("Unable to open JSON file", e.getStackTrace());
}

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;這裡要注意的是Path的JSON檔要放哪?
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;**是放在Deploy資料夾**

因為Filesystem.getDeployDirectory()其實就是在Resolve Deploy資料夾底下的資源路徑，所以Path就放在Deploy資料夾裡面再開一個Paths資料夾就好

4. 原型機的移動很不穩
其實主要是各種Constant的問題
像kS、kV、kA這種，影響超大，而且每台又差很多。
也是因為這樣從重新Characterize了一遍
新算的數據我放到discord了
之後如果需要更進階的Characterization我有用好的config可以跟我要

這四個就是主要遇到的問題，除此之外其實就是把Example改一改就好

### 其他好用資源
- [Pheonix系列Documentation ](https://store.ctr-electronics.com/documentation)(hardware和software都有)
- [Pheonix Software API 參照](https://store.ctr-electronics.com/content/api/java/html/index.html)(看class structure和method用的)
- [Pheonix Software Documentation ](https://docs.ctre-phoenix.com/en/stable/)(非class structure，有example的那種，比較接近WPILib的documentation)
- [WPI Lib API參照](https://first.wpi.edu/wpilib/allwpilib/docs/release/java/index.html) (class structure那種)

### 結論
Trajectory Following 做完之後應該就可以先開始進行路徑的實際規劃和非傳動馬達(e.g. arm) 的控制了
BTW Tele-op也需要修改一下，要符合Command structure
總的來說是有收穫的一天
想要看code的請到[此連結](https://github.com/FutureShock7130/2022-RapidReact-Robot-Code/tree/main/mecanumTrajectorySrc/main/java/frc/robot)

## 林俊彥
本日進度：麥克歐姆 來的時候冠穎請我讀懂學長的程式所以更改為
麥克歐姆+學長程式

**遇到問題**

從GITHUB下載下來的程式 import後的edu全被視為錯誤(已經修正 所以沒有照片) 一開始是推測路徑錯誤

**解決方法**

把SRC剪下 貼到一個全新PROJECT 並覆蓋

學長程式的翻譯[其實沒什麼好翻譯的ㄟ= =](https://hackmd.io/_mTVzLSSS62xOnFtSzRSuw)

### 吳玠廷

**1.今日進度**

* 到spark網站看spark的宣告以及驅動
* 跟學長學機器人路徑規劃並測試座標方向

**2. 遇到困難**

* 一開始機器人的port找不到(學長換了一個方法寫所以不是用id)
* 路徑規劃與機器人實際行走的路徑長有出入

**3. 解決方法**

* 後來port用盲猜猜到
* 後來調整了

# 20220208

## 陳伯豪

Today I am right on target.

### Work Progress

**1. At present, the MecanumDrive of the small robot has been completed. And test is also completed.**

### Problems

**1. Moter declaration should use 'WPI_TalonSRX' instead of PWMTalonSRX**

As Aaron mentioned above, our motor is through the CAN interface, so of course the objects used for PWM will not work.
Below is the declare code.

public class DriveSubsystem extends SubsystemBase {
  WPI_TalonSRX LFront = new WPI_TalonSRX(Constants.LFront_Port);
  WPI_TalonSRX LRear = new WPI_TalonSRX(Constants.LRear_Port);
  WPI_TalonSRX RFront = new WPI_TalonSRX(Constants.RFront_Port);
  WPI_TalonSRX RRear = new WPI_TalonSRX(Constants.RRear_Port);
  MotorControllerGroup LeftMotor = new MotorControllerGroup(LFront, LRear);
  MotorControllerGroup RightMotor = new MotorControllerGroup(RFront, RRear);
  MecanumDrive m_drive;
  double denominator;

**2. Lambda Expression**

When I am writing MecanumDrive of small robot, Albert mentions Lambda Expression. Therefore, I searched online for information about it. And I will briefly describe below.

The syntax of a lambda expression is as follows :

(parameters) -> expression

or 

(parameters) -> { statements; }

Lambda Expression has lots of features, Following are the important characteristics of Lambda Expressions

**One. Optional type declaration**

There is no need to declare the parameter type, 
the compiler can uniformly identify the parameter value.

**Two. Optional parameter parentheses**

A parameter does not need to define parentheses, 
but multiple parameters need to define parentheses.

**Three. Optional braces**

If the body contains a statement, 
curly braces are not required.

**Four. Optional return keyword**

If the body has only one expression return value, 
the compiler will automatically return the value. 

The curly brackets need to specify that the expression returns a value.

Lambda Expression Instance

// 1. No parameters are required, return 5
() -> 5  

// 2. Receives a parameter（number type） and return double its value.
x -> 2 * x  

// 3. Takes 2 int type numbers and returns their difference
(x, y) -> x – y  
 
// 4. Takes 2 int type numbers and returns their sum
(int x, int y) -> x + y

// 5. Take a string object, and prints it on the console, 
//    returns no value (looks like returns void)
(String s) -> System.out.print(s)

The above is a short explanation of lambda expressions, and below is The above is a short explanation of Lambda Expressions in MecanumDrive

// RobotContainer

public RobotContainer() {
  // Configure the button bindings
  configureButtonBindings();
  m_drive.setDefaultCommand(
    new RunCommand(
      () -> m_drive.setMotors( joystick.getRawAxis(1), joystick.getRawAxis(0), -joystick.getRawAxis(4)), m_drive)
      // The above function corresponds to setMotors in DriveSubsystem
    );
  }



// DriveSubsystem

//                            ↱ joystick.getRawAxis(1)    ↱ -joystick.getRawAxis(4)
public void setMotors( double ySpeed, double xSpeed, double zRotation ){
    // Mecanumderive                         ↳ joystick.getRawAxis(0)
    denominator = Math.max(Math.abs(ySpeed) + Math.abs(xSpeed) + Math.abs(zRotation), 1);
    LFront.set((ySpeed + xSpeed + zRotation) / denominator);
    RFront.set((ySpeed - xSpeed - zRotation) / denominator);
    LRear.set((ySpeed - xSpeed + zRotation) / denominator);
    RRear.set((ySpeed + xSpeed - zRotation) / denominator);
  }

#### Reference Website

* [Lambda Expression](https://www.oracle.com/webfolder/technetwork/tutorials/obe/java/Lambda-QuickStart/index.html#section2)
* [Lambda Expression in Runoob.com](https://www.runoob.com/java/java8-lambda-expressions.html)

### Videos

Here are some videos that I saw today while writing CommandBase and felt pretty good

* [FRC Official tutorial #5](https://youtu.be/VoxeXqy1bdQ)

:::spoiler Git ScreenShot Record


:::

## 王鴻霖 Aaron Wang

Today I experimented with what I call an auto aimming functionality, in which the robot will automatically rotate to the correct angle to aim at a field relative coordinate. This field relative coordinate should be changable so that it can be adjusted to suit different goals. On the other hand, Since the robot will be moving itself, I will also need to make it calculate the amount of rotation required at each point to aim at the target coordinate.

### Progress Summary

* create a new Command called teleopAimCommand that can be assigned to any joystick button as needed.
* Corrected the Encoder setting that was incorrect and causing wrong pose generation

### Challenges and Problem Solving Process

1. The rotation won't stop because the command never ends
**Reason**: Originally I used a Runcommand object directly configured to a joystick button for the auto aim function. At the (physical) end of the function I used the drive function from the DriveSubsystem to change the robot angle. However, since the Runcommand won't end unless interrupted, that was a failure and the rotation won't stop.
**Solution**: I created a new command class extending the CommandBase class. This kind of command will include initialize(), execute(), and most importantly the isFinished() that allows me to decide when the command should end. I then used a basic while loop inside the isFinished() function to determine whether the current angle is at the target angle. If it is, stops the rotation

2. Encoder returning incorrect value
**Reason**: Very simple typo. However, it has affected the result greatly due to the fact that I must use the correct current Pose to calculate the angle change required to rotate towards the target coordinate.
**Solution**: correct typo

3. After encoder returning correct value, rotation still incorrect
**Reason**: I shouldn't have relied to much on the libraries but more on my own math. Originally I was heavily replying on the `Pose2d`, `Translation2d`, and `Rotation2d` to calculate my final rotation. I misunderstood the Rotation2d constructor and though I can just input the translation difference in the x and y axis into it and found the correct rotation, which is wrong. In the end I drew some diagram on the white board and did some basic trigonometry to solve the problem. People who are interested in the final code can visit my [updated repository](https://github.com/FutureShock7130/2022-RapidReact-Robot-Code/tree/main/mecanumTrajectorySrc/main/java/frc/robot). I will add comments to the code to make it more easily understandable.

### Conclusion
Although today's progress is less important than that of yesterday, I think this is still an important step because now I understand how to make the robot correct its angle based on its position and the target position. This is to crucial recovering to trajectory and aiming at the goals to through in the balls.

## 黃冠穎
### 進度
**試著程式**

修改Aaron的程式讓我自己看得更懂，並嘗試加上encoder

**聽Aaron講解**

Aaron來之後我有請他講一下他的程式各個部分在幹嘛，有些官網資料有看過了有些則沒有

### 問題
**不知道加上encoder的程式能不能用**

如上所說，其他部份我要等學競考完再說了，我學競要爆了

# 20220209
## 林俊彥
本日進度： PID

> 某個仁：喔挖你好棒喔怎麼這麼厲害 (QQ)

## 謝亞諺
### 本日進度
1. 完善Limelight 的 Command base 程式
2. 使用git上傳程式碼到github([爛爛的程式](https://github.com/FUNDAI/mc))
3. 聽Aaron電神的Trajectory講解
### 問題
1. 在將Command綁定到按鈕時，出現未知的錯誤 
2. 在使用git的指令會把些步驟忘記
3. Aaron講解的時候有些高階的東東沒聽過
### 解決辦法
1. 使用Quick fixed(雖然沒有紅紅的東東，但不知道對不對)
2. 上網查教學
3. 詢問Aaron該怎用

# 20220210

# 20220212

## 魏仁祥

### 今日進度

**測試機器人**

協助劉宇幫和機械組測試機器人，其中有一個馬達的編號與其他的相同被蓋掉了一開始沒看到，後來全部都重新命名且修改過。rev spark max client 有問題，點進去的時候無法找到 spark。

**path weaver and syslid troubleshooting**

下午 Aaron 到的時候，他跟我們講他想試試利用場上的相對位置瞄準籃框的想法，以及詳細解釋他的程式在幹嘛。

:::spoiler TelopAimCmd Robot Code

package frc.robot.commands;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DriveTrainSubsystem;

public class teleopAimCommand extends CommandBase {
    /*
    This command is used to allow the robot to aim at a specific coordinate at any point on the field.
    Future update will make its target adjustable without changing the class itself. 
    */ 

    private DriveTrainSubsystem m_robotDrive;
    Translation2d initTranslation;
    Rotation2d initRotation;
    Rotation2d changeAngle; 
    Rotation2d finalRotation;
    Rotation2d currentRotation;
    double targetX = 3;
    double targetY = 4;
    
    public teleopAimCommand(DriveTrainSubsystem robotDrive){
            m_robotDrive = robotDrive;
            addRequirements(robotDrive);
    }
    
    @Override
    public void initialize(){
            initTranslation = m_robotDrive.getPose().getTranslation();
            initRotation = m_robotDrive.getPose().getRotation();
            // get translation2d object and rotation2d object from the Pose2d object from the DriveSubsysten.
            double xDiff = targetX - initTranslation.getX();
            // Calculate difference in x coordinate between the target coordinate and the current coordinate
            double yDiff = targetY - initTranslation.getY();
            // Calculate difference in y coordinate between the target coordinate and the current coordinate
            double changeCalc = Math.atan(yDiff/xDiff);
            // Calculate the angle difference between the two coordinate (if the current rotation(angle) is 0)
            changeAngle = new Rotation2d(changeCalc).plus(initRotation.times(-1));
            // Add the current angle into the rotation. Think of it as first correcting the angle of the robot to 0, 
            // then turn the actual angle difference between the two coordinates.
            changeAngle = changeAngle.getRadians()>Math.PI ? changeAngle.minus(new Rotation2d(2*Math.PI)) : changeAngle;
            // if the angle is greater than Pi, the angle is too big (larger than half the circle) 
            // and it's better to rotate the other way around. So I used this inline if statement (?: statement) to assign the angle
            // if the angle is greater than Pi, than it should turn the angle-2pi (draw a diagram to help you think about it)
            finalRotation = initRotation.rotateBy(changeAngle);
            // Calculate the final angle that the robot should be at, which will be used to determine when the rotation should stop
    }

    @Override
    public void execute(){
            m_robotDrive.drive(0,0, changeAngle.getRadians()/2, true);
            // divided by 2 to move slower but more precise
            currentRotation = m_robotDrive.getPose().getRotation();
            // update rotation for comparison
    }

    public boolean isFinished(){
            while (Math.abs(finalRotation.minus(currentRotation).getRadians())>0.05){
                    // if the current rotation is not 0.05 radians within the target angle, keep rotating
                    return false;
            }
            return true;
            // stops the function by returning true
    }
};

:::

不得不說，我覺得他很有想法。而且他也很實際地把程式寫出來，考慮到了所有事情，我根本不可能自己寫出這種東西。然而，由於這個方法過於理想，而場上的的碰撞與變因又真的太多且無法預測，這個我自己是暫時覺得當個備案是可以的，也應該主要是以 limelight 為主瞄準籃框吧。

接著與幫在走之前，他把機器人底盤都完成留給我們測試。我和 Aaron 和學弟即開始開啟 syslid 測試 ks kv ka，由於之前有測試過了小機器人，這次在測試上比較順

### 遇到問題

**測試機器人**


當我點進 rev spark max client 時，會出現如上圖的錯誤代碼
> Unexpected system error has occurred. Check log for details or contact support team.

**path weaver and syslid troubleshooting**

在拿到 ks kv ka 後，我們開始測試 path weaver，讓機器人跑出實際的路徑。然而，情況沒有這麼樂觀，機器人原地前後加速，跟原先預計的差很多很多。

### 如何解決

**測試機器人**

看到這個後，我跑去 rev spark max client 所放置的檔案裏面尋找 log 的文字檔，基本上有錯誤的話會自動生成一個錯誤代碼的 txt 黨在那邊才對，但我沒看到。

接著我去問范政佑，他是告訴我要用線接筆電和 spark，但我自己還沒嘗試。我是比較好奇難道以後每次都要用線接嗎？這樣很浪費時間也很麻煩，我自己是覺得有其他的方法例如設置　IP address 之類的，這還要再看看。

**path weaver and syslid troubleshooting**

我不知道，我後來就高歌了

### 結論

**測試機器人**

待測試，我當天後來就沒試了。

**path weaver and syslid troubleshooting**

高歌

# 20220213

## 謝亞諺
### 今日進度
1. 被警衛趕出來
2. 等公車
### 問題
1. 排班表在搞ㄟ
### 解決辦法
1. 以後排班到星期日別來