Last active
January 29, 2023 19:09
-
-
Save DeltaDizzy/1191cc0bdcf0206b69161869053415bc to your computer and use it in GitHub Desktop.
LTVDifferentialDriveController repro code
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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