1. 配置Talon SRX
用 USB 连接 RoboRIO,在浏览器中访问http://172.22.11.2/#Home
,可以配置 Talon SRX 的 CAN id
2. 接线
CAN 总线的连接方式是 RoboRIO --> Talon 1 --> Talon 2 --> ... --> PDP
3. PID 调试
以下代码
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package org.usfirst.frc.team5451.robot;
import com.ctre.phoenix.motorcontrol.*;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.XboxController;
public class Robot extends IterativeRobot {
private XboxController joystick = new XboxController(0);
private TalonSRX controller2 = new TalonSRX(10);
private TalonSRX controller1 = new TalonSRX(11);
public static final int kPIDLoopIdx = 0;
public static final int kTimeoutMs = 30;
@Override
public void robotInit() {
controller1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, kPIDLoopIdx, kTimeoutMs);
controller1.setSensorPhase(false);
controller1.setInverted(true);
controller1.configNominalOutputForward(0, kTimeoutMs);
controller1.configNominalOutputReverse(0, kTimeoutMs);
controller1.configPeakOutputForward(1, kTimeoutMs);
controller1.configPeakOutputReverse(-1, kTimeoutMs);
// PID 参数就调这里
controller1.config_kF(kPIDLoopIdx, 0.00, kTimeoutMs);
controller1.config_kP(kPIDLoopIdx, 0.05, kTimeoutMs);
controller1.config_kI(kPIDLoopIdx, 0.0003, kTimeoutMs);
controller1.config_kD(kPIDLoopIdx, 0.0003, kTimeoutMs);
}
@Override
public void teleopPeriodic() {
double leftYstick = joystick.getY(Hand.kLeft);
double motorOutput = controller1.getMotorOutputPercent();
if (joystick.getAButton()) {
/* Speed mode */
/* Convert 500 RPM to units / 100ms.
* 4096 Units/Rev * 500 RPM / 600 100ms/min in either direction:
* velocity setpoint is in units/100ms
*/
/* 500 RPM in either direction */
double targetVelocity_UnitsPer100ms = 500.0 * 4096 / 600;
// 编码器读值含义: num = rpm * 4096 / 600
controller1.set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);
SmartDashboard.putNumber("Sensor Velocity", targetVelocity_UnitsPer100ms);
/* append more signals to print when in speed mode. */
} else {
/* Percent voltage mode */
controller1.set(ControlMode.PercentOutput, 0.5 * leftYstick);
}
SmartDashboard.putNumber("Voltage", controller1.getMotorOutputVoltage());
SmartDashboard.putNumber("getSelectedSensorVelocity", controller1.getSelectedSensorVelocity(kPIDLoopIdx));
SmartDashboard.putNumber("motorOutput", motorOutput);
SmartDashboard.putNumber("CloseLoopError", controller1.getClosedLoopError(kPIDLoopIdx));
}
}
编码器读值含义: num = rpm * 4096 / 600
网友评论