Swerve turn and spin

Technical Programming
#2

What do you mean it won’t spin and move? Does it only do one or the other? Do those functions work separately?

Posting your code would probably help us debug with you.

1 Like
#3

Could you specify? Do you have video or specific footage? Can you upload your code?

There isn’t much we can do without any information.

#4

it will spin and it will move but not at the same time

#5

here is the code

/----------------------------------------------------------------------------/
/* Copyright © 2017-2018 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;

//imports
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.subsystems.;
import frc.robot.commands.;
import frc.robot.OI;
//imports

/**

  • The VM is configured to automatically run this class, and to call the
  • functions corresponding to each mode, as described in the TimedRobot
  • documentation. If you change the name of this class or the package after
  • creating this project, you must also update the build.gradle file in the
  • project.
    */
    public class Robot extends TimedRobot {

// public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
// public static OI m_oi;

Command m_autonomousCommand;
Joystick _joy = new Joystick(0);
Joystick _backupJoy = new Joystick(1);
SendableChooser m_chooser = new SendableChooser<>();
// command and subsystem calls
public static DriveSubsystem drive = null;
public static LifterSubsystem lift = null;
public static WristSubsystem wrist = null;
public static IntakeSubsystem intake = null;
public static PnumaticSubsystem pnumatics = null;
// up subsystems down commands
public static SwerveDrive swerve = null;
public static GrabBall grabBall = null;
public static GrabDisk grabDisk = null;
public static shift shift = null;
public static scoredisk releaseDisk = null;
public static scoreball releaseBall = null;
public static manualWrist manualWrist = null;
public static manualLift manualLift = null;
public static lifter_test lifter = null;
boolean ran = false;

// command and subsystem calls
/**

  • This function is run when the robot is first started up and should be used
  • for any initialization code.
    */
    @Override
    public void robotInit() {
// command and subsystem calls
drive = new DriveSubsystem();
lift = new LifterSubsystem();
wrist = new WristSubsystem();
intake = new IntakeSubsystem();
pnumatics = new PnumaticSubsystem();
// up subsystems down commands
releaseBall = new scoreball();
releaseDisk = new scoredisk();
swerve = new SwerveDrive();
grabBall = new GrabBall();
grabDisk = new GrabDisk();
shift = new shift();
manualWrist = new manualWrist();
manualLift = new manualLift();
lifter = new lifter_test();

drive.init();

// command and subsystem calls

// m_oi = new OI();
// m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
// chooser.addOption("My Auto", new MyAutoCommand());
// SmartDashboard.putData("Auto mode", m_chooser);

}

/**

  • This function is called every robot packet, no matter the mode. Use this for
  • items like diagnostics that you want ran during disabled, autonomous,
  • teleoperated and test.

  • This runs after the mode specific periodic functions, but before LiveWindow
  • and SmartDashboard integrated updating.
    */
    @Override
    public void robotPeriodic() {
    if (_joy.getRawButton(13) || _backupJoy.getRawButton(13)) {
    drive.init();
    }
    if (_joy.getRawButton(7) || _backupJoy.getRawButton(7)) { // on press ready and spinning on releace ball is stowed
    grabBall.start();
    }
    if (_joy.getRawButton(4) || _backupJoy.getRawButton(4)) { // on press ready to grab hatch on releace grab hatch
    grabDisk.start();
    }
    if (_joy.getRawButton(11) || _backupJoy.getRawButton(11)) { // on press shift to low on releace shift to high
    pnumatics.shiftlow();
    }
    else {
    pnumatics.shifthigh();
    }
    if (_joy.getRawButton(6) || _backupJoy.getRawButton(6)) { // on press claws closed on relace claws open
    releaseDisk.start();
    }
    if (_joy.getRawButton(8) || _backupJoy.getRawButton(8)) { // on press shoot ball
    releaseBall.start();
    }
    if (_backupJoy.getRawButton(1) || _backupJoy.getRawButton(3)) { // square button wrist up circle wrist up
    manualWrist.start();
    }
    if (_backupJoy.getRawButton(2) || _backupJoy.getRawButton(4)) { // triangle button lift up x button lift down
    manualLift.start();
    }
    if (_joy.getRawButton(1)) {
    lifter.start();
    }
    swerve.start();
    }

/**

  • This function is called once each time the robot enters Disabled mode. You
  • can use it to reset any subsystem information you want to clear when the
  • robot is disabled.
    */
    @Override
    public void disabledInit() {
    }

@Override
public void disabledPeriodic() {
Scheduler.getInstance().run();
}

/**

  • This autonomous (along with the chooser code above) shows how to select
  • between different autonomous modes using the dashboard. The sendable chooser
  • code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
  • remove all of the chooser code and uncomment the getString code to get the
  • auto name from the text box below the Gyro

  • You can add additional auto modes by adding additional commands to the
  • chooser code above (like the commented example) or additional comparisons to
  • the switch structure below with additional strings & commands.
    */
    @Override
    public void autonomousInit() {
    m_autonomousCommand = m_chooser.getSelected();
/*
 * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
 * switch(autoSelected) { case "My Auto": autonomousCommand = new
 * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
 * ExampleCommand(); break; }
 */

// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
  m_autonomousCommand.start();
}

}

/**

  • This function is called periodically during autonomous.
    */
    @Override
    public void autonomousPeriodic() {
    Scheduler.getInstance().run();
    }

@Override
public void teleopInit() {

// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
  m_autonomousCommand.cancel();
}

}

/**

  • This function is called periodically during operator control.
    */
    @Override
    public void teleopPeriodic() {
    Scheduler.getInstance().run();
    }

/**

  • This function is called periodically during test mode.
    */
    @Override
    public void testPeriodic() {
    }
    }
#6

So that’s your Robot class, which references a bunch of subsystems with the specific code we care about and need to see to help you. Is your code on Github or a similar hosting platform so we could see all of it?

#7

oh whoops i copyed the wrong one sorry

/----------------------------------------------------------------------------/
/* Copyright © 2018 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 edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.;
import com.ctre.phoenix.motorcontrol.can.;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import frc.robot.OI;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SPI;
import com.kauailabs.navx.frc.AHRS;

/**

  • Add your docs here.
    */

public class DriveSubsystem extends Subsystem {
AHRS ahrs;
Joystick _joy = new Joystick(0);
double radians, angle, temp, A2, B2, R, A, B, C, D, ws1, ws2, ws3, ws4, wa1, wa2, wa3, wa4, max, currentAngle,
rotationAmmount, FWD, STR, RCW, currentAngle2, currentAngle3, currentAngle4, rotationAmmount2,
rotationAmmount3, rotationAmmount4; // defining variables formulas

// defining motor controlers and encoders
WPI_TalonSRX SRXsteer = new WPI_TalonSRX(OI.SRXtoprightsteer);
WPI_TalonSRX SRXsteer2 = new WPI_TalonSRX(OI.SRXtopleftsteer);
WPI_TalonSRX SRXsteer3 = new WPI_TalonSRX(OI.SRXbottomleftsteer);
WPI_TalonSRX SRXsteer4 = new WPI_TalonSRX(OI.SRXbottomrightsteer);
CANSparkMax MAXdrive = new CANSparkMax(OI.MAXtoprightdrive, MotorType.kBrushless);
CANSparkMax MAXdrive2 = new CANSparkMax(OI.MAXtopleftdrive, MotorType.kBrushless);
CANSparkMax MAXdrive3 = new CANSparkMax(OI.MAXbottomleftdrive, MotorType.kBrushless);
CANSparkMax MAXdrive4 = new CANSparkMax(OI.MAXbottomrightdrive, MotorType.kBrushless);
public CANPIDController drive1 = new CANPIDController(MAXdrive);
public CANEncoder Encoder = new CANEncoder(MAXdrive);
// defining motor controlers and encoders

@Override
public void initDefaultCommand() {
	MAXdrive.setMotorType(MotorType.kBrushless); // defining motor type
}

public void init() {
	// init for navX imu
	ahrs = new AHRS(SPI.Port.kMXP);
	ahrs.resetDisplacement();
	ahrs.reset();
	// init for navX imu

	// PID config
	drive1.setP(1);
	drive1.setI(0);
	drive1.setD(0);

	int kTimeoutMs = 10;
	int kPIDLoopIdx = 0;

	int absolutePosition = SRXsteer.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;

	SRXsteer.setSelectedSensorPosition(absolutePosition, kPIDLoopIdx, kTimeoutMs);
	SRXsteer.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
	SRXsteer.setSensorPhase(false);
	SRXsteer.selectProfileSlot(0, kPIDLoopIdx);
	SRXsteer.configNominalOutputForward(0, kTimeoutMs);
	SRXsteer.configNominalOutputReverse(0, kTimeoutMs);
	SRXsteer.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer.configPeakOutputReverse(-1, kTimeoutMs);
	SRXsteer.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
	SRXsteer.configMotionCruiseVelocity(15000, kTimeoutMs);
	SRXsteer.configMotionAcceleration(16000, kTimeoutMs);

	SRXsteer.config_kF(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer.config_kP(kPIDLoopIdx, 1, kTimeoutMs);
	SRXsteer.config_kI(kPIDLoopIdx, 0, kTimeoutMs);
	SRXsteer.config_kD(kPIDLoopIdx, 0, kTimeoutMs);
	SRXsteer.setSelectedSensorPosition(0, 0, 0);

	int absolutePosition2 = SRXsteer2.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;

	SRXsteer2.setSelectedSensorPosition(absolutePosition2, kPIDLoopIdx, kTimeoutMs);
	SRXsteer2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
	SRXsteer2.setSensorPhase(false);

	SRXsteer2.configNominalOutputForward(0, kTimeoutMs);
	SRXsteer2.configNominalOutputReverse(0, kTimeoutMs);
	SRXsteer2.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer2.configPeakOutputReverse(-1, kTimeoutMs);
	SRXsteer2.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
	SRXsteer2.configMotionCruiseVelocity(15000, kTimeoutMs);
	SRXsteer2.configMotionAcceleration(16000, kTimeoutMs);

	SRXsteer2.config_kF(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer2.config_kP(kPIDLoopIdx, 1, kTimeoutMs);
	SRXsteer2.config_kI(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer2.config_kD(kPIDLoopIdx, 0, kTimeoutMs);
	SRXsteer2.setSelectedSensorPosition(0, 0, 0);

	int absolutePosition3 = SRXsteer3.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;

	SRXsteer3.setSelectedSensorPosition(absolutePosition3, kPIDLoopIdx, kTimeoutMs);
	SRXsteer3.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
	SRXsteer3.setSensorPhase(false);

	SRXsteer3.configNominalOutputForward(0, kTimeoutMs);
	SRXsteer3.configNominalOutputReverse(0, kTimeoutMs);
	SRXsteer3.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer3.configPeakOutputReverse(-1, kTimeoutMs);
	SRXsteer3.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
	SRXsteer3.configMotionCruiseVelocity(15000, kTimeoutMs);
	SRXsteer3.configMotionAcceleration(16000, kTimeoutMs);

	SRXsteer3.config_kF(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer3.config_kP(kPIDLoopIdx, 1, kTimeoutMs);
	SRXsteer3.config_kI(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer3.config_kD(kPIDLoopIdx, 0, kTimeoutMs);
	SRXsteer3.setSelectedSensorPosition(0, 0, 0);

	int absolutePosition4 = SRXsteer4.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;

	SRXsteer4.setSelectedSensorPosition(absolutePosition4, kPIDLoopIdx, kTimeoutMs);
	SRXsteer4.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
	SRXsteer4.setSensorPhase(false);

	SRXsteer4.configNominalOutputForward(0, kTimeoutMs);
	SRXsteer4.configNominalOutputReverse(0, kTimeoutMs);
	SRXsteer4.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer4.configPeakOutputReverse(-1, kTimeoutMs);
	SRXsteer4.configMotionCruiseVelocity(15000, kTimeoutMs);
	SRXsteer4.configMotionAcceleration(16000, kTimeoutMs);

	SRXsteer4.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);

	SRXsteer4.config_kF(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer4.config_kP(kPIDLoopIdx, 1, kTimeoutMs);
	SRXsteer4.config_kI(kPIDLoopIdx, 0.0, kTimeoutMs);
	SRXsteer4.config_kD(kPIDLoopIdx, 0, kTimeoutMs);
	SRXsteer4.setSelectedSensorPosition(0, 0, 0);

	// PID config

}

public void Swerve() {
	angle = ahrs.getYaw(); // defining variable for gyro
	radians = angle * Math.PI / 180;
	currentAngle = SRXsteer.getSelectedSensorPosition(0) / 25.930555555555; // setting the current angle of the
																			// wheel 11.4666 = tick per rotation/360
	currentAngle2 = SRXsteer2.getSelectedSensorPosition(0) / 25.930555555555;
	currentAngle3 = SRXsteer3.getSelectedSensorPosition(0) / 25.930555555555;
	currentAngle4 = SRXsteer4.getSelectedSensorPosition(0) / 25.930555555555;
	// swerve formulas
	FWD = -_joy.getRawAxis(1);
	STR = _joy.getRawAxis(0);
	RCW = _joy.getRawAxis(2);
	if (FWD < .08 && FWD > -.08) {
		FWD = 0;
	}
	if (STR < .08 && STR > -.08) {
		STR = 0;
	}
	if (RCW < .08 && RCW > -.08) {
		RCW = 0;
	}
	temp = FWD * Math.cos(radians) + STR * Math.sin(radians);
	STR = -FWD * Math.sin(radians) + STR * Math.cos(radians);
	FWD = temp;
	R = Math.sqrt((OI.L * OI.L) + (OI.W * OI.W));
	A = STR - RCW * (OI.L / R);
	B = STR + RCW * (OI.L / R);
	C = FWD + RCW * (OI.W / R);
	D = FWD + RCW * (OI.W / R);
	A2 = STR + RCW * (OI.L / R);
	B2 = STR - RCW * (OI.L / R);
	ws1 = Math.sqrt((B2 * B2) + (C * C));
	ws2 = Math.sqrt((B * B) + (D * D));
	ws3 = Math.sqrt((A * A) + (D * D));
	ws4 = Math.sqrt((A2 * A2) + (C * C));
	wa1 = Math.atan2(B2, C) * 180 / Math.PI;
	wa2 = Math.atan2(B, D) * 180 / Math.PI;
	wa3 = Math.atan2(A, D) * 180 / Math.PI;
	wa4 = Math.atan2(A2, C) * 180 / Math.PI;
	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;
	}
	if (RCW != 0) {
		ws1 = -ws1;
		ws4 = -ws4;
	}

	// swerve formulas

	// smartdashboard puts

	// smartdashboard puts
	SmartDashboard.putNumber("Encoder4 Position", SRXsteer4.getSelectedSensorPosition(0));
	SmartDashboard.putNumber("Encoder3 Position", SRXsteer3.getSelectedSensorPosition(0));
	SmartDashboard.putNumber("Encoder Position", SRXsteer.getSelectedSensorPosition(0));
	SmartDashboard.putNumber("Encoder2 Position", SRXsteer2.getSelectedSensorPosition(0));

	SmartDashboard.putNumber("angle4", currentAngle4);
	SmartDashboard.putNumber("output4", rotationAmmount4);
	SmartDashboard.putNumber("trtwt4", wa4 - currentAngle4);
	SmartDashboard.putNumber("angle3", currentAngle);
	SmartDashboard.putNumber("output3", rotationAmmount3);
	SmartDashboard.putNumber("trtwt3", wa4 - currentAngle3);

	SmartDashboard.putNumber("WheelAngle", wa1);
	SmartDashboard.putNumber("WheenSpeed", ws1);
	SmartDashboard.putNumber("WheelAngle2", wa2);
	SmartDashboard.putNumber("WheenSpeed2", ws2);
	SmartDashboard.putNumber("WheelAngle3", wa3);
	SmartDashboard.putNumber("WheenSpeed3", ws3);
	SmartDashboard.putNumber("WheelAngle4", wa4);
	SmartDashboard.putNumber("WheenSpeed4", ws4);
	SmartDashboard.putNumber("RotaitonAmount", rotationAmmount);
	SmartDashboard.putNumber("current", SRXsteer4.getOutputCurrent());
	SmartDashboard.putNumber("angle", angle);
	SmartDashboard.putNumber("joyasi1", _joy.getRawAxis(1));
	SmartDashboard.putNumber("joyasi0", _joy.getRawAxis(0));
	SmartDashboard.putNumber("joyasi2", _joy.getRawAxis(2));
	// System.out.println(rotationAmmount);

	rotationAmmount = Math.IEEEremainder(wa1 - currentAngle, 360); // calculating ammount to move wheel
	rotationAmmount2 = Math.IEEEremainder(wa2 - currentAngle2, 360); // calculating ammount to move wheel
	rotationAmmount3 = Math.IEEEremainder(wa3 - currentAngle3, 360); // calculating ammount to move wheel
	rotationAmmount4 = Math.IEEEremainder(wa4 - currentAngle4, 360); // calculating ammount to move wheel

	MAXdrive.setRampRate(.25);
	MAXdrive2.setRampRate(.25);
	MAXdrive3.setRampRate(.25);
	MAXdrive4.setRampRate(.25);

	if (FWD < .1 && FWD > -.1 && STR < .1 && STR > -.1 && RCW < .1 && RCW > -.1) { // not letting the wheels move
																					// unitll the joystick is pushed
																					// %10 down
		MAXdrive.set(0);
		MAXdrive2.set(0);
		MAXdrive3.set(0);
		MAXdrive4.set(0);
	} else {
		MAXdrive.set(-ws1); // drining drive wheel off the formulas
		MAXdrive2.set(ws2);
		MAXdrive3.set(ws3);
		MAXdrive4.set(-ws4);
	}

//	if (FWD < .08 && FWD > -.08 && STR < .08 && STR > -.08 && RCW < .08 && RCW > -.08) { // deadband
//	} else {
		SRXsteer.set(ControlMode.MotionMagic, (currentAngle + rotationAmmount) * 26.006); // moving the wheel to the
		// required position
		SRXsteer2.set(ControlMode.MotionMagic, (currentAngle2 + rotationAmmount2) * 26.006); // moving the wheel to
																								// the
		// required position
		SRXsteer3.set(ControlMode.MotionMagic, (currentAngle3 + rotationAmmount3) * 26.006); // moving the wheel to
																								// the
		// required position
		SRXsteer4.set(ControlMode.MotionMagic, (currentAngle4 + rotationAmmount4) * 26.006); // moving the wheel to
																								// the
		// required position
//	}

	/*
	 * SRXsteer.configPeakCurrentLimit(25, 10);
	 * SRXsteer.configPeakCurrentDuration(200, 10);
	 * SRXsteer.configContinuousCurrentLimit(10, 10);
	 * SRXsteer.enableCurrentLimit(true); SRXsteer2.configPeakCurrentLimit(25, 10);
	 * SRXsteer2.configPeakCurrentDuration(200, 10);
	 * SRXsteer2.configContinuousCurrentLimit(10, 10);
	 * SRXsteer2.enableCurrentLimit(true); SRXsteer3.configPeakCurrentLimit(25, 10);
	 * SRXsteer3.configPeakCurrentDuration(200, 10);
	 * SRXsteer3.configContinuousCurrentLimit(10, 10);
	 * SRXsteer3.enableCurrentLimit(true); SRXsteer4.configPeakCurrentLimit(25, 10);
	 * SRXsteer4.configPeakCurrentDuration(200, 10);
	 * SRXsteer4.configContinuousCurrentLimit(10, 10);
	 * SRXsteer4.enableCurrentLimit(true);
	 */

	SmartDashboard.putNumber("ANGLE", ahrs.getAngle());
}

public void driveForward(double distance) {
	currentAngle = SRXsteer.getSelectedSensorPosition(0) / 26.006; // setting the current angle of the wheel 11.4666
																	// = tick per rotation/360
	rotationAmmount = Math.IEEEremainder(0 - currentAngle, 360); // calculating ammount to move wheel
	SRXsteer.set(ControlMode.Position, (currentAngle + rotationAmmount) * 26.006); // moving the wheel to the
																					// required position
	drive1.setReference(distance, ControlType.kPosition); // moving drive to target position
}

}

#8

So what is it doing specifically? Can you describe the behavior we are trying to diagnose?

#9

It looks like the way you are setting deadband for steering causes the steering motors to not turn when you are driving above 8%.

#10

The mechanical guys are working on it right now so i cant send a video but when we try to move and spin at the same time it gets confused and does weird things

#11

What does weird things mean? Do the motors start spinning wildly? Have you tried using printouts or even better something like Shuffleboard to map the expected state without moving a physical object?

#12

the wheels face eachother and try to spin in oppsite directions causing the robot not to move

#13

Try adding a system that prints out the desired angle and speed of each wheel and try running the code without running the hardware. That will narrow it down to either a controls problem or a feedback problem.

#14

we have 2 deadbands can you refer to which one you mean?

#15

I mean, where you would normally instruct the motors to move to a position, add a line that says:

System.out.println("Wheel 1 Desired Angle: " + (position you would tell it to turn to));
System.out.println("Wheel 1 Desired Speed: " + (velocity you would instruct the wheel to go to));

and so on and so forth for all of the wheels. By doing this we can determine if the problem is code or feedback.

#16

that question was directed to asid61 sorry for the confusion

#17

It sounds like you likely have a sign error somewhere, or have mis-assigned your wheel positions in code. There’s no way I’m going to be able to debug it with what you’ve shared though, because it is a mess. Please factor your code through judicious use of subroutines and data-holding objects, and put it on a standard platform such as github so it can be easily read.

1 Like
#18

here are the results
when pushing forward on drive joystick and right on spinning we get the following
Wheel angle 1: -23
Wheel angle 2: 23
Wheel angle 3: -23
Wheel angle 4: 23
Wheel speed 1: %100
Wheel speed 2: %100
Wheel speed 3: %100
Wheel speed 4: %100

#19

What is the layout of the wheels?

12
34

or

14
23

or something else?

Also I have to concur with Oblarg. If you could clean this up and put it on Git that would be really helpful for people trying to helpful, but I’ll stick with you for the sake of your swerve drive.

#20

will do

#21

here you go i put the drive subsystem on there