Skip to content

Instantly share code, notes, and snippets.

@DeltaDizzy
Last active January 29, 2023 19:09
Show Gist options
  • Save DeltaDizzy/1191cc0bdcf0206b69161869053415bc to your computer and use it in GitHub Desktop.
Save DeltaDizzy/1191cc0bdcf0206b69161869053415bc to your computer and use it in GitHub Desktop.
LTVDifferentialDriveController repro code
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.LTVDifferentialDriveController;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends TimedRobot {
LTVDifferentialDriveController ltv;
@Override
public void robotInit() {
Timer t = new Timer();
SmartDashboard.putString("InitStatus", "RobotInit");
System.out.println("Timer Starting");
t.start();
ltv = new LTVDifferentialDriveController(
LinearSystemId.identifyDrivetrainSystem(
0.00052047,
0.00036209,
0.00051414,
7.7103E-05,
0.014367
),
0.014367,
// states = x pos, y pos, angle, left speed, right speed
VecBuilder.fill(0.1, 0.1, 0.1, 0.5, 0.5),
// inputs = left volts, right volts
VecBuilder.fill(7.0, 7.0),
0.02
);
SmartDashboard.putString("InitStatus", "LTV Started");
SmartDashboard.putNumber("LTV Time", t.get());
t.stop();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment