Can I do Trajectory following with Timed Robot Base

There is limited time and I still want to push for a smooth path in auto

The short answer is yes, technically. (Anything is possible)

The longer answer, I believe you would have to look at the implementation of the RamseteCommand and RamseteController and port the relevant code across.

That is my initial thoughts.

Thank you, can you post some documentation

Upon closer inspection, you may just be able to use the RamseteController in Timed Robot.
I can’t tell for sure as I’m only on my phone. I’ll have a look tomorrow if someone hasn’t already beat me to it.

https://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/controller/RamseteController.html

1 Like

You can take a look at our code for that this season:

/----------------------------------------------------------------------------/

/* Copyright © 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 frc.robot;

import java.util.ArrayList;

import com.ctre.phoenix.motorcontrol.ControlMode;

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

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.Encoder;

import edu.wpi.first.wpilibj.Joystick;

import edu.wpi.first.wpilibj.LinearFilter;

import edu.wpi.first.wpilibj.controller.PIDController;

import edu.wpi.first.wpilibj.controller.RamseteController;

import edu.wpi.first.wpilibj.SerialPort;

import edu.wpi.first.wpilibj.TimedRobot;

import edu.wpi.first.wpilibj.Timer;

import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;

import edu.wpi.first.wpilibj.geometry.Pose2d;

import edu.wpi.first.wpilibj.geometry.Rotation2d;

import edu.wpi.first.wpilibj.geometry.Translation2d;

import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;

import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;

import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;

import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import edu.wpi.first.wpilibj.trajectory.Trajectory;

import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;

import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;

import edu.wpi.first.wpilibj.util.Units;

/**

  • The VM is configured to automatically run this class, and to call the

  • functions corresponding to each mode, as described in the TimedRobot

  • documentation. If you change the name of this class or the package after

  • creating this project, you must also update the build.gradle file in the

  • project.

*/

public class Robot extends TimedRobot {

Joystick ps4 = new Joystick(0);

// Joystick leftJoystick = new Joystick(1);

// Joystick rightJoystick = new Joystick(2);

TalonSRX leftFront = new TalonSRX(3);

TalonSRX leftBack = new TalonSRX(1);

TalonSRX rightFront = new TalonSRX(4);

TalonSRX rightBack = new TalonSRX(2);

Encoder leftEncoder = new Encoder(1, 0);

Encoder rightEncoder = new Encoder(3, 2);

AHRS ahrs = new AHRS(SerialPort.Port.kMXP);

Timer timer = new Timer();

/**

  • This function is run when the robot is first started up and should be

  • used for any initialization code.

*/

@Override

public void robotInit() {

leftBack.set(ControlMode.Follower, leftFront.getDeviceID());

rightBack.set(ControlMode.Follower, rightFront.getDeviceID());



leftFront.enableVoltageCompensation(true);

leftFront.configVoltageCompSaturation(12);

leftBack.enableVoltageCompensation(true);

leftBack.configVoltageCompSaturation(12);

rightFront.enableVoltageCompensation(true);

rightFront.configVoltageCompSaturation(12);

rightBack.enableVoltageCompensation(true);

rightBack.configVoltageCompSaturation(12);

leftEncoder.setDistancePerPulse(-0.5 * Math.PI / 2048.0 * 0.3048);

rightEncoder.setDistancePerPulse(0.5 * Math.PI / 2048.0 * 0.3048);

ahrs.zeroYaw();

}

/**

  • This function is called every robot packet, no matter the mode. Use

  • this for items like diagnostics that you want ran during disabled,

  • autonomous, teleoperated and test.

  • This runs after the mode specific periodic functions, but before

  • LiveWindow and SmartDashboard integrated updating.

*/

@Override

public void robotPeriodic() {

}

/**

  • This autonomous (along with the chooser code above) shows how to select

  • between different autonomous modes using the dashboard. The sendable

  • chooser code works with the Java SmartDashboard. If you prefer the

  • LabVIEW Dashboard, remove all of the chooser code and uncomment the

  • getString line to get the auto name from the text box below the Gyro

  • You can add additional auto modes by adding additional comparisons to

  • the switch structure below with additional strings. If using the

  • SendableChooser make sure to add them to the chooser code above as well.

*/

@Override

public void autonomousInit() {

}

/**

  • This function is called periodically during autonomous.

*/

@Override

public void autonomousPeriodic() {

}

DifferentialDriveOdometry odometry;

Trajectory trajectory;

RamseteController controller;

@Override

public void teleopInit() {

timer.start();

ahrs.zeroYaw();

leftEncoder.reset();

rightEncoder.reset();

odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(ahrs.getYaw()));

Pose2d start  = new Pose2d(Units.feetToMeters(0), Units.feetToMeters(0), Rotation2d.fromDegrees(0));

Pose2d end = new Pose2d(Units.feetToMeters(15), Units.feetToMeters(7.5), Rotation2d.fromDegrees(0));

ArrayList<Translation2d> interior = new ArrayList<Translation2d>();

interior.add(new Translation2d(Units.feetToMeters(5), Units.feetToMeters(-4)));

interior.add(new Translation2d(Units.feetToMeters(10), Units.feetToMeters(4)));

// interior.add(new Translation2d(Units.feetToMeters(5), Units.feetToMeters(5)));

// interior.add(new Translation2d(Units.feetToMeters(0), Units.feetToMeters(5)));

TrajectoryConfig config = new TrajectoryConfig(2.5, 2);

// config.setReversed(true);

trajectory = TrajectoryGenerator.generateTrajectory(start, interior, end, config);

controller = new RamseteController(2.0, 0.7);

}

LinearFilter filter = LinearFilter.singlePoleIIR(0.1, 0.02);

LinearFilter filter2 = LinearFilter.singlePoleIIR(0.1, 0.02);

SimpleMotorFeedforward leftFeedForward = new SimpleMotorFeedforward(0.09, 0.33);

SimpleMotorFeedforward rightFeedForward = new SimpleMotorFeedforward(0.09, 0.37);

PIDController leftPID = new PIDController(0.5, 0.15, 0);

PIDController rightPID = new PIDController(0.7, 0.2, 0);

DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(2 * 0.3048);

/**

  • This function is called periodically during operator control.

*/

@Override

public void teleopPeriodic() {

double leftCommand = -ps4.getRawAxis(1) * 3 * 0.5, rightCommand = -ps4.getRawAxis(3) * 3 * 0.5 ;

double leftRawSpeed = leftEncoder.getRate();

double rightRawSpeed = rightEncoder.getRate();

double leftSpeed = filter.calculate(leftRawSpeed);

double rightSpeed = filter2.calculate(rightRawSpeed);

Pose2d pose = odometry.update(Rotation2d.fromDegrees(-ahrs.getYaw()), leftEncoder.getDistance(), rightEncoder.getDistance());

Trajectory.State goal = trajectory.sample(timer.get());

ChassisSpeeds adjustedSpeeds = controller.calculate(pose, goal);

DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);

// leftCommand = wheelSpeeds.leftMetersPerSecond;

// rightCommand = wheelSpeeds.rightMetersPerSecond;

if(Math.abs(leftCommand) < 0.05 / 3.0/0.5) leftCommand = 0;

if(Math.abs(rightCommand) < 0.05 / 3.0 / 0.5) rightCommand = 0;

tankDrive(leftFeedForward.calculate(leftCommand) + leftPID.calculate(leftSpeed, leftCommand), 

          rightFeedForward.calculate(rightCommand) + rightPID.calculate(rightSpeed, rightCommand));

// tankDrive(leftCommand, rightCommand);

SmartDashboard.putNumber("robotTime", timer.get());

SmartDashboard.putNumber("setpoint", leftCommand);

SmartDashboard.putNumber("speed", leftSpeed);

SmartDashboard.putNumber("distance", ahrs.getYaw());

}

public void tankDrive(double leftSpeed, double rightSpeed) {

leftFront.set(ControlMode.PercentOutput, leftSpeed);

rightFront.set(ControlMode.PercentOutput, -rightSpeed);

}

/**

  • This function is called periodically during test mode.

*/

@Override

public void testPeriodic() {

}

}

2 Likes

Please put your code in triple back-ticks so it gets formatted ```, or better yet, provide a link to GitHub.

This code looks like it would drive a trajectory in teleop. I think you want to do the trajectory in auto and let the driver control the bot in teleop.

1 Like