I have a differential drive robot custom odometry and pid bad working

sipmine · May 6, 2022, 3:59am

i use my own odometry and when i connect the pid to drive by coordinates it goes fine only for x if you connect y then it just goes forward
my code

github.com

sipmine/RobotL/blob/master/FollowTwo/src/main/java/frc/robot/subsystems/ExampleSubsystem.java#:~:text=public void velocity(double x_W, double y_W, double theta_W) {

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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.subsystems;

import com.kauailabs.navx.frc.AHRS;
import com.studica.frc.TitanQuad;
import com.studica.frc.TitanQuadEncoder;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.SPI;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

This file has been truncated. show original

bovlb · May 6, 2022, 4:27am

I haven’t looked at everything, but here are two things that leap out at me:

In the definition of rightLenV and leftLenV, you don’t include any gear ratio, and you seem to be forgetting a factor of 2 (c=2 * pi * r).
You use the value of the member variable theta, which is only set in getTheta, which is never called.

sipmine · May 6, 2022, 4:53am

Get theta called on commands. rightLenV and leftLenV use this formula Encoders — Studica Robotics 1.0.0 documentation