FTC_Textbooks

实战演练——五轴机械臂控制程序

简介

五轴机械臂，故名思议，具有五个自由度加上末端夹子，所以需要六个舵机。我们依次给它们编号，从距离夹子最远的舵机开始为0号，夹子舵机就是5号。

那么这五个不控制夹子的舵机分别干什么呢？

机械臂

首先，0号舵机，控制整个机械臂相对于车的角度。

就像这样：

      (车 头)
         y
     \   |         
      \  |         
       \ |
        \|
---------+---------x
         |         
         |         
         |
         |
      (车 尾)

其次，1号舵机控制机械臂第一臂相对于水平面的角度。

然后，2号舵机控制第一臂和第二臂的夹角。

接着，3号舵机使得你可以控制夹子（在机械臂在水平面上的投影与z轴相交形成的面上）的朝向。

4号舵机使得夹子可以绕着它的朝向旋转。

最后，5号舵机控制夹子开合。

综上，夹子的定位需要使用0-4舵机，开合控制需要使用5号舵机。

编写代码

基础功能

首先，由于舵机有着0-1的控制方式，并且每个舵机具体的角度均不同；

所以，我们应当编写一个功能，将角度值转化为舵机值，反之亦然。

为此，我们需要有：

double[] servoZeroPositionDegree = new double[5];
double[] servoRangeDegree = new double[5];

通过这两组数值，我们可以创建一个返回舵机数值的函数：

double toPosition(double servoIndex, double Degree){
  return ( Degree - servoZeroPositionDegree[servoIndex] ) / servoRangeDegree[servoIndex];
}

同样地：

//区别于Math库中的同名方法，库中的方法需要写出 库名.toDegree，而该处方法不用。
double toDegree(double servoIndex, double Position){
  return Position * servoRangeDegree[servoIndex] + servoZeroPositionDegree[servoIndex];
}

为了控制舵机，我们需要对舵机进行初始化。

我们需要先：

Servo servo = new Servo[6];

然后从hardwareMap获取：

servo[0] = hardwareMap.get(Servo.class,"servoe0");
servo[1] = hardwareMap.get(Servo.class,"servoe1");
servo[2] = hardwareMap.get(Servo.class,"servoe2");
servo[3] = hardwareMap.get(Servo.class,"servoe3");
servo[4] = hardwareMap.get(Servo.class,"servoe4");
servo[5] = hardwareMap.get(Servo.class,"servoe5");

然后，设置舵机的正反转：

servo[0].setDirection(Servo.Direction.FORWARD);
servo[1].setDirection(Servo.Direction.FORWARD);
servo[2].setDirection(Servo.Direction.REVERSE);
servo[3].setDirection(Servo.Direction.REVERSE);
servo[4].setDirection(Servo.Direction.REVERSE);
servo[5].setDirection(Servo.Direction.FORWARD);

这样，我们就可以通过

servo[n].setPosition(pos);

来输出舵机数值。

我们把这些功能放进一个名为SixServoArmController.java的文件中：

package org.firstinspires.ftc.teamcode.Controllers.SixServoArm;
//用作为其他Class提供函数的Class
public class SixServoArmController{
  private HardwareMap hardwareMap;//硬件地图
  private Telemetry telemetry;//遥测

  public SixServoArmController(ardwareMap hardwareMap, Telemetry telemetry){
    this.hardwareMap=hardwareMap;
    this.telemetry=telemetry;
  }//构造函数
  private static SixServoArmController instance; //实例，用以单例模式
  public static SixServoArmController getInstance(){
    return instance;
  }//获取单例，无论何时何地都是同一个
  public static void setInstance(HardwareMap hardwareMap, Telemetry telemetry){
    instance = new SixServoArmController(hardwareMap,telemetry);
  }//更新单例，使得每次启动OpMode都能够更新

}
//用作输出的Class
class SixServoArmOutputer{
  private HardwareMap hardwareMap;
  private Telemetry telemetry;
  public SixServoArmOutputer(ardwareMap hardwareMap, Telemetry telemetry){
    this.hardwareMap=hardwareMap;
    this.telemetry=telemetry;
  }//构造函数
}
//用作三角函数计算的Class

FTC_Textbooks

实战演练——五轴机械臂控制程序

目录

简介

编写代码

基础功能