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?
the wheels face eachother and try to spin in oppsite directions causing the robot not to move
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.
we have 2 deadbands can you refer to which one you mean?
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.
that question was directed to asid61 sorry for the confusion
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
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
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.
will do
here you go i put the drive subsystem on there
rotationAmmount = Math.IEEEremainder(wa1 - currentAngle, 360);
// calculating ammount to move wheel
SRXsteer4.set(ControlMode.MotionMagic, (currentAngle4 + rotationAmmount4) * 26.006);
Why are you doing this? Why not just convert the wa values output by Oblarg’s Swerve Math into TalonSRX units and tell Motion Magic to go directly to that? Also, what is * 26.006 doing there?
the Math.IEEEEremainder is so if we want to move from 359 to 0 it dosent do a 360 and the 26.006 is the number on encoder ticks per rotation divided by 360
Are you sure about that?
360*26.006 = 9362.16
360*26.006 = 13.8429593171
What encoder are you using? The TalonSRX Mag Encoder has a resolution of 4096 bits in Quadrature Mode and 1024 bits in Pulse Width mode, just for reference. Almost every encoder I’ve ever seen has a number of ticks along the lines of 2^x of a multiple of 10. This seems to be a non-sensible number, but I could be wrong.
we have a second gear out of our gearbox with the encoder in it which is why the number is diffrent
What is the gear ratio? Also that’s probably not a great idea since you’ll face significantly increased backlash, and on a system as sensitive and fragile as a Swerve Drive, that can be extremely dangerous.
Thank you for putting the code on github. You still really do need to refactor it, because it is extremely unreadable in its present state.
Regardless, it looks like at least one problem is that either line 180 or 181 contains a sign error. I’ve included a diagram to help you figure out the kinematics:
Edit: to avoid confusion, please note that ‘w’, which is what you seem to have named your robot width in your code (and I have put on the diagram accordingly) is not the same as omega, which appears in the equations on the right-hand side and stands for the rotational velocity. Also, I omitted a factor of R (oops), but it is very clear where it should go.
That worked great thank you so much for your help!
In order to spin and translate at the same time, you can’t have all 4 wheels spinning at 100% speed. If the robot is spinning clockwise and translating forward, the wheels on the left side of the robot need to go faster than the right side of the robot. If your normal max translation speed uses 100% wheel speed you will not be able to spin while translating at max speed. You may want to try setting your maximum wheel speed lower (at least while you de-bug your code). Start off at 50 or 60% until you get things working. After you have it working you will probably be able to increase it, but you probably want to keep it at 80% or less of max speed to still give you some extra speed to translate.