Weird swerve drive issue

FIRST
, , , , ,
1

Hello, our team has been working on a swerve drive robot and we started to have some weird issues with the motion of the robot. The bot is using MK4i modules and is being controlled with an Xbox controller. The issue is that when moving the robot in a circle without rotating the robot the wheels will do one revolution and then start to move in the wrong direction.

The robot is supposed to move in a circle but instead after moving around the circle once the modules switch direction which causes the motion to look like this:
actual motion vid

The modules are supposed to move continuously in a circle but instead after completing one revolution, they switch direction. This works properly if the rotation optimizer is commented out but the wheels do not rotate efficiently and switch direction a lot. It also works properly if the field-oriented conversions are commented out and in this case, the wheels move continuously and smoothly.

Robot.h:


#pragma once

#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <ctre/Phoenix.h>
#include <iostream>
#include <frc/Timer.h>
#include <string>
#include <frc/Solenoid.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include "frc/smartdashboard/Smartdashboard.h"
#include "networktables/NetworkTable.h"
#include "networktables/NetworkTableInstance.h"
#include "networktables/NetworkTableEntry.h"
#include "networktables/NetworkTableValue.h"
#include <chrono>
#include <thread>
#include <math.h>
#include <cameraserver/CameraServer.h>
#include "AHRS.h"
//#include "frc/span"



class Robot : public frc::TimedRobot {
 public:
  void RobotInit() override;
  void RobotPeriodic() override;

  void AutonomousInit() override;
  void AutonomousPeriodic() override;

  void TeleopInit() override;
  void TeleopPeriodic() override;

  void DisabledInit() override;
  void DisabledPeriodic() override;

  void TestInit() override;
  void TestPeriodic() override;

  //**********************Drive Loop***************
  void drive(double FWD, double STR, double RCW){
    yaw = ahrs->GetYaw();
    if(yaw<0){
      yaw += 360; 
    }

    yaw = (yaw * (M_PI/180));
//**************FOC conversions**************************
    double temp = ((FWD * cos(yaw)) + (STR * sin(yaw)));
    STR = ((-FWD) * sin(yaw)) + (STR * cos(yaw));
    FWD = temp;
    std::cout<< "FWD "<<FWD<<" STR "<<STR<<" RCW "<< RCW <<std::endl;



    //********Setting ratios for wheel distance********
    L = 25.75;
	  W = 25.75;

	  R = sqrt((L * L) + (W * W));

	  A = STR - RCW * (L / R);
	  B = STR + RCW * (L / R);
	  C = FWD - RCW * (W / R);
	  D = FWD + RCW * (W / R);




    //********Set wheel angles and speeds********
	  ws1 = sqrt((B * B) + (C * C));
	  wa1 = atan2(B, C) * 180 / M_PI;
    wa1 *= conversionFactor;

	  ws2 = sqrt((B * B) + (D * D));
	  wa2 = atan2(B, D) * 180 / M_PI;
    wa2 *= conversionFactor;

	  ws3 = sqrt((A * A) + (D * D));
	  wa3 = atan2(A, D) * 180 / M_PI;
    wa3 *= conversionFactor;

	  ws4 = sqrt((A * A) + (C * C));
	  wa4 = atan2(A, C) * 180 / M_PI;
    wa4 *= conversionFactor;




    //*********Optimize wheel angles********
    angleOptimize(oldwa1,wa1);
    wa1=outputAngle;
    ws1*=direction;

    angleOptimize(oldwa2,wa2);
    wa2=outputAngle;
    ws2*=direction;

    angleOptimize(oldwa3,wa3);
    wa3=outputAngle;
    ws3*=direction;

    angleOptimize(oldwa4,wa4);
    wa4=outputAngle;
    ws4*=direction;



    //********Set max speed********
	  double max = ws1;
	  if (ws2 > max)max = ws2;
	  if (ws3 > max)max = ws3;
	  if (ws4 > max)max = ws4;
	  if (max > 1) { ws1 /= max; ws2 /= max; ws3 /= max; ws4 /= max; }




    //********Set angle motor position********
    m_rra.Set(TalonFXControlMode::Position, wa4);
    m_fra.Set(TalonFXControlMode::Position, wa1);
    m_rla.Set(TalonFXControlMode::Position, wa3);
    m_fla.Set(TalonFXControlMode::Position, wa2);




    //********Set old angles********
    oldwa1 = wa1;
    oldwa2 = wa2;
    oldwa3 = wa3;
    oldwa4 = wa4;




    //********Drive Speed!********
    double PercentOut = -0.3; // must be negative


    //********Set Drive Motors********
    m_frd.Set(PercentOut*ws1);
    m_fld.Set(PercentOut*ws2);
    m_rld.Set(PercentOut*ws3);
    m_rrd.Set(PercentOut*ws4);
  }                             

  *********************Angle Optimization Algorithm*************************
  void angleOptimize(double current, double desired){
	  direction = 1;
	  angle1 = (desired - (fmod(current,4096)));
  	angle2 = ((2048 - abs(fmod(desired,4096))) + (2048 - abs(fmod(current,4096))));

  	if (abs(angle1) <= abs(angle2)) {
  		shortest = angle1;
  		check1 = false;
  	}
  	else {
  		shortest = angle2;
  		check1 = true;
  	}

	angle1 = ((desired - 2048) - fmod(current,4096));
	angle2 = ((2048 - (abs(fmod(desired,4096))-2048)) + (2048 - (abs(fmod(current,4096)))));
	
	if (abs(angle2) < abs(angle1)) {
		shortest2 = angle2;
		check2 = true;
	}
	else {
		shortest2 = angle1;
		check2 = false;
	}

	if (abs(shortest) <= abs(shortest2)) {
		if (check1 == true) {
			if (current < 0) {
				current -= shortest;
			}
			else {
				current += shortest;
			}
		}
		else {
			current += shortest;
		}
	}
	else {
		direction = -1;
		if (check2 == true) {
			if (current < 0) {
				current -= shortest2;
			}
			else {
				current += shortest2;
			}
		}
		else {
			current += shortest2;
		}
	}
  outputAngle = current;
  }

  private:
    //***********************CONTROLLERS********************************
    frc::XboxController m_driverController{0};

    //**************MOTORS AND ENCODERS********************************
    WPI_TalonFX m_fra{9};
    WPI_TalonFX m_frd{11};

    WPI_TalonFX m_rra{12};
    WPI_TalonFX m_rrd{10};

    WPI_TalonFX m_fla{0};
    WPI_TalonFX m_fld{5};

    WPI_TalonFX m_rla{15};
    WPI_TalonFX m_rld{18};


    WPI_CANCoder m_rrsensor{5};
    WPI_CANCoder m_frsensor{2};
    WPI_CANCoder m_rlsensor{1};
    WPI_CANCoder m_flsensor{3};

    //*******************SWERVE DRIVE VARIABLES*****************************
    double conversionFactor = 4096.0/ 360.0;

    double L;
    double W;
    double R;

	  double A;
    double B;
    double C;
    double D;

    double ws1;
    double wa1;
    double ws2;
    double wa2;
    double ws3;
    double wa3;
    double ws4;
    double wa4;

    double oldwa1;
    double oldwa2;
    double oldwa3;
    double oldwa4;

    //**************************NAVX-MXP (Gyro board)**********************
    AHRS *ahrs;
    double yaw;

    //*********************Swerve Drive Optimization************************
    double direction;
    double angle1;
  	double angle2;
  	double shortest;
  	bool check1;
    double shortest2;
	  bool check2;

    double outputAngle;

  };

Robot.cpp:


#include "Robot.h"

void Robot::RobotInit() {
  m_rra.SetSensorPhase(true);
  m_fra.SetSensorPhase(true);
  m_rla.SetSensorPhase(true);
  m_fla.SetSensorPhase(true);
  m_rra.SetNeutralMode(NeutralMode::Brake);
  m_fra.SetNeutralMode(NeutralMode::Brake);
  m_rla.SetNeutralMode(NeutralMode::Brake);
  m_fra.SetNeutralMode(NeutralMode::Brake);




  //*******************Configure Rear Right Angle Motor********************************
  m_rra.ConfigFactoryDefault();
  m_rra.ConfigRemoteFeedbackFilter(5, RemoteSensorSource(13), 0, 0);
  m_rra.ConfigSelectedFeedbackSensor(FeedbackDevice::RemoteSensor0, 0, 0); // PIDLoop=0, timeoutMs=0

  m_rra.Config_kP(0, 1.7);
  m_rra.Config_kI(0, .0016);
  m_rra.Config_kD(0, 160);
  m_rra.Config_kF(0, 0);
  m_rra.Config_IntegralZone(0, 20);

  m_rra.ConfigNominalOutputForward(0);
	m_rra.ConfigNominalOutputReverse(0);
	m_rra.ConfigPeakOutputForward(1);
	m_rra.ConfigPeakOutputReverse(-1);




  //*******************Configure Front Right Angle Motor*******************************
  m_fra.ConfigFactoryDefault();
  m_fra.ConfigRemoteFeedbackFilter(2, RemoteSensorSource(13), 0, 0);
  m_fra.ConfigSelectedFeedbackSensor(FeedbackDevice::RemoteSensor0, 0, 0); // PIDLoop=0, timeoutMs=0

  m_fra.Config_kP(0, 1.7);
  m_fra.Config_kI(0, .0016);
  m_fra.Config_kD(0, 160);
  m_fra.Config_kF(0, 0);
  m_fra.Config_IntegralZone(0, 20);

  m_fra.ConfigNominalOutputForward(0);
	m_fra.ConfigNominalOutputReverse(0);
	m_fra.ConfigPeakOutputForward(1);
	m_fra.ConfigPeakOutputReverse(-1);




  //*******************Configure Rear Left Angle Motor*********************************
  m_rla.ConfigFactoryDefault();
  m_rla.ConfigRemoteFeedbackFilter(1, RemoteSensorSource(13), 0, 0);
  m_rla.ConfigSelectedFeedbackSensor(FeedbackDevice::RemoteSensor0, 0, 0); // PIDLoop=0, timeoutMs=0

  m_rla.Config_kP(0, 1.7);
  m_rla.Config_kI(0, .0016);
  m_rla.Config_kD(0, 160);
  m_rla.Config_kF(0, 0);
  m_rla.Config_IntegralZone(0, 20);

  m_rla.ConfigNominalOutputForward(0);
	m_rla.ConfigNominalOutputReverse(0);
	m_rla.ConfigPeakOutputForward(1);
	m_rla.ConfigPeakOutputReverse(-1);




  //*************Configure Front Left Angle Motor********************************
  m_fla.ConfigFactoryDefault();
  m_fla.ConfigRemoteFeedbackFilter(3, RemoteSensorSource(13), 0, 0);
  m_fla.ConfigSelectedFeedbackSensor(FeedbackDevice::RemoteSensor0, 0, 0); // PIDLoop=0, timeoutMs=0

  m_fla.Config_kP(0, 1.7);
  m_fla.Config_kI(0, .0016);
  m_fla.Config_kD(0, 160);
  m_fla.Config_kF(0, 0);
  m_fla.Config_IntegralZone(0, 20);

  m_fla.ConfigNominalOutputForward(0);
	m_fla.ConfigNominalOutputReverse(0);
	m_fla.ConfigPeakOutputForward(1);
	m_fla.ConfigPeakOutputReverse(-1);




  //**************Configure Cancoder Magnet Offsets**************************
  m_flsensor.ConfigMagnetOffset(100);
  m_frsensor.ConfigMagnetOffset(110);
  m_rlsensor.ConfigMagnetOffset(250);
  m_rrsensor.ConfigMagnetOffset(148);
  m_flsensor.SetPositionToAbsolute();
  m_frsensor.SetPositionToAbsolute();
  m_rlsensor.SetPositionToAbsolute();
  m_rrsensor.SetPositionToAbsolute();




  //**********************Configure the gyro board****************************
  ahrs = new AHRS(frc::I2C::Port::kMXP);

}
void Robot::RobotPeriodic() {}

void Robot::AutonomousInit() {}
void Robot::AutonomousPeriodic() {}

void Robot::TeleopInit() {
  //timer.Reset();
  //timer.Start();
}
void Robot::TeleopPeriodic() {
  double forward = -m_driverController.GetRightY();
  double strafe =  m_driverController.GetRightX();
  double rotate = m_driverController.GetLeftX();

  //********Controller Deadzones********
  if((forward<.2)&&(forward>-.2)){forward = 0;}
  if((strafe<.2)&&(strafe>-.2)){strafe = 0;}
  if((rotate<.2)&&(rotate>-.2)){rotate = 0;}
  
  drive(forward, strafe, rotate);
}

void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}

void Robot::TestInit() {}
void Robot::TestPeriodic() {}

#ifndef RUNNING_FRC_TESTS
int main() {
  return frc::StartRobot<Robot>();
}
#endif

This is the link to our GitHub which has additional videos that CD wouldn’t let me include: GitHub - LovelandHighRobotics1977/Whiplash: Code for Whiplash, team 1977's 2023 charged up FRC robot
Any information or suggestions would be greatly appreciated!

2 Likes
2

I havent looked through the code yet, but i would bet money that it has something to do with encoder angle rolling over from 360->0 (or vice-versa)

3 Likes
3

The angleOptimize method is very complex. I cannot understand what it is doing. Our code is Java, but it’s only a few lines of code.

We use SwerveModuleState.optimize() to handle when reversing the wheel direction is more efficient:

var optimizedState = SwerveModuleState.optimize(moduleState, getSteerAngle());

Then we deal with the fact that the Talon/Falcon encoder is continuous:

    // The reference angle has the range [0, 2pi) but the Falcon's encoder can go
    // above that
    double adjustedReferenceAngleRadians = desiredAngleRadians + currentAngleRadians - currentAngleRadiansMod;
    if (desiredAngleRadians - currentAngleRadiansMod > Math.PI) {
      adjustedReferenceAngleRadians -= 2.0 * Math.PI;
    } else if (desiredAngleRadians - currentAngleRadiansMod < -Math.PI) {
      adjustedReferenceAngleRadians += 2.0 * Math.PI;
    }

For reference, here’s a link to our code

4

Thanks! I kept working on the code and by testing and debugging different parts of it I figured out that it was because the angle being fed into the optimizer was being modded by 4096 but this did not give the correct value for the actual angle.

5

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