---
# System prepended metadata

title: 昱翔 - 程式組的機器人工作順序
tags: [程式組教程]

---

# 昱翔 - 程式組的機器人工作順序
## 一、先弄好底盤移動  


|       | 右前 | 右後 | 左前 | 左後 |
| ----- | ---- | ---- | ---- | ---- |
|talon| 2 |3| 1 | 4 |
|方向|?轉  | ?轉  | ?轉  | ?轉  |


### 用內建diffirentDrive 有三種
```java=
// ArcadeDrive   (double xSpeed,double zRotation, boolean squaredInputs)
 differentialDrive.arcadeDrive(leftStick_Y,-rightStick_X,true);
// TankDrive      (double leftSpeed,double rightSpeed,boolean squaredInputs)
 differentialDrive.arcadeDrive(leftStick_Y,rightStick_X,true);
// curvatureDrive   (double xSpeed,double zRotation,boolean isQuickTurn)
 differentialDrive.curvatureDrive(leftStick_Y,-rightStick_X,true);
```
## 二、收球


| 吸球 | 底轉盤 |
| ---- | ------ | 
| 12  | 13   | 
| ?轉  |        | 
### 程式碼 功能:按下按鈕，吸球跟底轉盤同時轉
```java=
if(js.getRawButton(Btn_B)){
    mCollector.set(ControlMode.PercentOutput,0.5);
    mConveyer.set(ControlMode.PercentOutput, 0.5);
}else{
    mCollector.set(ControlMode.PercentOutput,0);
    mConveyer.set(ControlMode.PercentOutput, 0);
}
```

## 三、射球
### 射球機構編號
| 砲台轉盤 | 仰角 | 射球(MAIN)|射球(副)|射球輸送|
| ---- | ---- | ---- | ---- |---- |
| 10 | 8 |  5 |6|7|
| ?轉  | ?轉  | ?轉  || | 

### 用phoenix tuner 用 percentage 測試射球裝置能力

|           | 距離 | Velocity | 敞篷encoder |
| --------- | ---- | -------- | ----------- |
| 調pid位置  |      |          |             |
| 最遠      |      |          |             |
| 最近      |      |          |             |

### 調射球PID

|                | 射球   | 轉盤 | 敞篷 |
| -------------- | ------ | ---- | ---- |
| I Zone         | 300    |      |      |
| kD             | 55     |      |      |
| kF             | 0      |      |      |
| kI             | 0.0023 |      |      |
| kP             | 1.7    |      |      |
| Loop Period Ms | 1      |      |      |
| Max Integral   | 0      |      |      |
| Peak Output    | 1      |      |      |

## 四、設定LimeLight
### 調顏色
[LimeLight調完色連結](https://drive.google.com/file/d/1ub2880tftVQc2Q4Ws7WhXRsd29OvKfuE/view?usp=sharing)
### 程式碼
```java=
//Initialize
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
    NetworkTableEntry tx = table.getEntry("tx");
    NetworkTableEntry ty = table.getEntry("ty");
    NetworkTableEntry ta = table.getEntry("ta");
//Period
double x = tx.getDouble(0.0);
double y = ty.getDouble(0.0);
double area = ta.getDouble(0.0);
SmartDashboard.putNumber("LimelightX", x);
SmartDashboard.putNumber("LimelightY", y); SmartDashboard.putNumber("LimelightArea", area);
```
## 五、轉盤自動對正程式
必須先設定好LimeLight跟轉盤PID

```java=
int turret =30; //砲台旋轉對影像的轉換
int nw_pos=aimHori.getSelectedSensorPosition();
aimHori.set(ControlMode.Position,nw_pos+x*turret);
```
## 六、測距離
1.f(x)=-0.01x^7 -0.11x^6 -0.55x^5 -1.76x^4 -3.04x^3 -1.17x^2 -4.09x+184.96
2.f(x)=-0.02x^3 -0.25x^2-9.39x+181.87

![](https://i.imgur.com/WOZYU58.jpg)

```javascript=
    double  forDis=-0.01*Math.pow(y,7)-0.11*Math.pow(y,6)-0.55*Math.pow(y,5)-1.76*Math.pow(y,4)-3.04*Math.pow(y,3)-1.17*Math.pow(y,2)-4.09*y+184.96;

```
## 七、氣壓
資源:https://github.com/wpilibsuite/frc-docs/blob/master/source/docs/software/actuators/pneumatics.rst

### 電磁閥編號 
| 底盤 | 收球 | 抬升 |
| ---- | ---- | ---- |
| 0&1 | 2&3 | 4&5 |

### 程式碼
``` java=
//intialize
Compressor compressor = new Compressor(0);
Solenoid gearbox = new Solenoid(-1);
Solenoid sCollector = new Solenoid(-1);
Solenoid sLift = new Solenoid(-1);
//period
if(js2.getRawButton(Btn_A))
      gearbox.set(true);
if(js2.getRawButton(Btn_B))
      gearbox.set(false);
if(js2.getRawButton(Btn_X))
      sCollector.set(true);
if(js2.getRawButton(Btn_Y))
      sCollector.set(false);
```

## 八、抬升
### 抬升馬達編號
| 抬升(with encoder) | 抬升(no encoder) |
|:------------------:|:----------------:|
|         11         |        9         |

## Encoder 程式碼

```java=
//encoder 反轉
motor.setSensorPhase(true)
```
![](https://i.imgur.com/knoPHGb.png)
![](https://i.imgur.com/Ay1njAL.png)
![](https://i.imgur.com/X4UtUvC.png)

## 九、搖桿
![](https://i.imgur.com/eRayHBR.png)

## 十、Limit Switch 


| Column 1 | Column 2 | Column 3 |     |     |
| -------- | -------- | -------- | --- | --- |
| Text     | Text     | Text     |     |     |



## 搖桿功能
Js1:
 arcadeDrive：
 - X:抬升左移
 - Y:抬升上拉
 - A:抬升下移
 - B:抬升右移
 - LS_Btn:氣壓變速
 - LB:抬升氣壓
 - RB:按:底盤功率1 不按:底盤功率0.5

Js2:
- X:氣壓收球 收球合一
- Trigger_L:吸球+旋轉
- B:射球+旋轉
- LB:射球轉換 自動/手動 模式
- LS_Y:敞篷調整
- RS_X:砲台調整
    
## 底盤數據
輪子直徑:15.748

###### tags: `程式組教程`


