WPILib MecanumDriveKinematics angular velocity not usable

FRC6302 · March 6, 2022, 6:46pm

Hello, I am trying to use the wpilib MecanumDriveKinematics class to get the angular velocity on our robot, but I am getting unusable/inconsistent values. It works for getting vx and vy, so I don’t think it is a hardware issue.

In my drivetrain subsystem periodic method, I have this line

curChassisSpeeds = kinematics.toChassisSpeeds(getCurrentWheelSpeeds());

and then I have a method that returns

Math.toDegrees(curChassisSpeeds.omegaRadiansPerSecond);

Here are the graphs on SmartDashboard after rotating in place and driving a little bit to test vx and vy: (I’m pretty sure the time axes on each graph aren’t synced btw)

I can use the NavX gyro to get the actual angular velocity and it works, and you can compare the gyro and mecanum angular velocity graphs to see how the mecanum one is bad.

I am wondering if this is something wrong with my code or something wrong in the wpilib libaries

My drivetrain subsystem and full code is here

github.com

FRC6302/2022Bot1/blob/master/src/main/java/frc/robot/subsystems/MecDriveTrain.java

// 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.subsystems;


import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.SparkMaxAlternateEncoder.Type;

import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.UnscentedKalmanFilter;
import edu.wpi.first.math.geometry.Pose2d;

This file has been truncated. show original

Oblarg · March 6, 2022, 8:09pm

Nothing here looks totally out of the ordinary. Derivatives of measured data are noisy - often to the point where you can’t use them without a fair amount of clever filtering.

If you have a known good signal from the gyro, you should delegate to that.

FRC6302 · March 6, 2022, 9:00pm

Oh. Then why are vx and vy so much better?

Oblarg · March 6, 2022, 9:17pm

Those are first-order differences; angular velocity is a second-order difference.

system · March 6, 2023, 9:18pm

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.