help with mecanum drive code

[austin1743]

Hello, I am doing some off-season coding practice but I keep getting an error that i have been stumped on, Can someone help me out? just some tips or a way to go about it.

So far i have:


// things below this need to be imported first
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import com.sun.squawk.util.MathUtils;

public class RobotTemplate extends IterativeRobot {
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
RobotDrive m_robotDrive;// robot will use PWM 1-4 for drive motors

//declears speed controllers
Victor leftFrontspeed = new Victor(4, 1);
Victor rightFrontspeed = new Victor(4, 2);
Victor leftBackspeed = new Victor(4,3);
Victor rightBackspeed = new Victor(4,4);
DriverStation driverStation; // driver station object
Dashboard dashboard;

// Declare variables for the two joysticks being used
Joystick js1; // joystick 1 (arcade stick or right tank stick)

public class RobotTemplate extends IterativeRobot {
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
RobotDrive m_robotDrive;// robot will use PWM 1-4 for drive motors
public void teleopPeriodic() {
// leftspeed.set(js1.getY());
// rightspeed.set(js1.getY());

//is called when teloporated mode is enabled
double lf;//is what will be set to the left wheel speed
double rf;//is what will be set to the right wheel speed
double lb; // is what will be set to the left back wheel speed
double rb; // is what will be set to the left back wheel speed
double y = js1.getY(); //gets the value of the 1'st joystick's y axis
double x = js1.getX();//gets the value of the 1'st joystick's x axis
//y = y * Math.abs(y);//makes the y value quadratic
//x = x * Math.abs(x);//makes the x value quadratic
x=-x;
//begins meckanum code which I do not understand but it works
double magnitude =Math.sqrt(js1.getY()*js1.getY()+js1.getX()*js1.ge tX());
magnitude = magnitude*Math.sqrt(2);
double rotation = js1.getTwist();
double direction=MathUtils.atan2(x, y);
double dirInRad = (direction+45);
double sinD=Math.sin(dirInRad);
double cosD=Math.cos(dirInRad)*Math.sqrt(2);
lf=-sinD*magnitude+rotation;
rf=-cosD*magnitude-rotation;
lb=-cosD*magnitude+rotation;
rb=-sinD*magnitude-rotation;
//ends mecanum code

//lf=y;
//lb=y;
//rb=y;
//rf=y;

//runs function limit to ensure the speed of the motors is in exceptable range
lf =limit(lf,1,-1);
lb =limit(lb,1,-1);
rf =limit(rf,1,-1);
rb =limit(rb,1,-1);

//sets the motors speed
leftFrontspeed.set(lf);
leftBackspeed.set(lb);
rightFrontspeed.set(-rf);
rightBackspeed.set(-rb);

System.out.println("js1.y:"+js1.getY()+"\t js1.x:"+js1.getX()+"\t lf:"+lf+"\t rf:"+rf+"\t lb:"+lb+"\t rb:"+rb);

}

}

I get the errors saying that the following lines of code are "Cannot find symbol". I do what netbeans wants me to do and it makes a class for it but then i get a lot more errors.

If someone has just a standard drive code i can look at please refer me to that link.

thanks a lot.

[Joe Ross]

Where is js1 declared?

[austin1743]

thanks, i didn't even catch that, i had it in my first code document but not in the second...

// things below this need to be imported first
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import com.sun.squawk.util.MathUtils;


public class RobotTemplate extends IterativeRobot {
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
RobotDrive m_robotDrive;// robot will use PWM 1-4 for drive motors

//declears speed controllers
Victor leftFrontspeed = new Victor(4, 1);
Victor rightFrontspeed = new Victor(4, 2);
Victor leftBackspeed = new Victor(4,3);
Victor rightBackspeed = new Victor(4,4);
DriverStation driverStation; // driver station object
Dashboard dashboard;

// Declare variables for the two joysticks being used
Joystick js1; // joystick 1 (arcade stick or right tank stick)


public void robotInit() {

}


/**This function is called periodically during operator control
*/
public void teleopPeriodic() {
// leftspeed.set(js1.getY());
// rightspeed.set(js1.getY());

//is called when teloporated mode is enabled
double lf;//is what will be set to the left wheel speed
double rf;//is what will be set to the right wheel speed
double lb; // is what will be set to the left back wheel speed
double rb; // is what will be set to the left back wheel speed
double y = js1.getY(); //gets the value of the 1'st joystick's y axis
double x = js1.getX();//gets the value of the 1'st joystick's x axis
//y = y * Math.abs(y);//makes the y value quadratic
//x = x * Math.abs(x);//makes the x value quadratic
x=-x;
//begins meckanum code which I do not understand but it works
double magnitude =Math.sqrt(js1.getY()*js1.getY()+js1.getX()*js1.ge tX());
magnitude = magnitude*Math.sqrt(2);
double rotation = js1.getTwist();
double direction=MathUtils.atan2(x, y);
double dirInRad = (direction+45);
double sinD=Math.sin(dirInRad);
double cosD=Math.cos(dirInRad)*Math.sqrt(2);
lf=-sinD*magnitude+rotation;
rf=-cosD*magnitude-rotation;
lb=-cosD*magnitude+rotation;
rb=-sinD*magnitude-rotation;
//ends mecanum code

//lf=y;
//lb=y;
//rb=y;
//rf=y;

//runs function limit to ensure the speed of the motors is in exceptable range
lf =limit(lf,1,-1);
lb =limit(lb,1,-1);
rf =limit(rf,1,-1);
rb =limit(rb,1,-1);



//sets the motors speed
leftFrontspeed.set(lf);
leftBackspeed.set(lb);
rightFrontspeed.set(-rf);
rightBackspeed.set(-rb);

System.out.println("js1.y:"+js1.getY()+"\t js1.x:"+js1.getX()+"\t lf:"+lf+"\t rf:"+rf+"\t lb:"+lb+"\t rb:"+rb);


}

}

I have put in the js1 (joystick 1). I have also highlighted (in bold) where I am getting my error at.

[JosephC]

Mecanum in a nutshell

magnitude = leftStick.getMagnitude();
direction = leftStick.getDirectionDegrees();
rotation = rightStick.getX();
driveRobot.mecanumDrive_Polar(magnitude, direction, rotation);

[austin1743]

Ross, i have added the js1 (joystick1) into the code thanks for catching that

Joseph thanks for the mecanum in a nut shell. I know mecanum wheels have some weird math vectors behind them which i sort of understand but how does that relate to the controller? I ask this as I am a little confused on why the code would have magnitude from the controller?

[Ether]

Ross, i have added the js1 (joystick1) into the code thanks for catching that

Joseph thanks for the mecanum in a nut shell. I know mecanum wheels have some weird math vectors behind them which i sort of understand but how does that relate to the controller? I ask this as I am a little confused on why the code would have magnitude from the controller?

If the driver is using a joystick, the magnitude can be computed using sqrt(X^2+Y^2), or perhaps max(abs(X),abs(Y)). That is used as a speed command - how fast the driver wants the vehicle to go.

The direction is computed using for example theta = atan2(-Y,X). It is the clockwise angle with zero in the forward direction. It is the driver command to tell which direction the driver wants the vehicle to move.

[austin1743]

thanks Ether for the explanation.

I am going to try the code out soon to see if it works on our robot.

[austin1743]

Ok well, unforntanetly i gotta wait to test this on our robot....

But i have come across some errors in the code.

The error i am getting is on: Public class Robot extends IterativeRobot {
is says that a class or interference expected..

you can see the java code in the google doc at:
https://docs.google.com/document/d/1eC7B3J3wUOIKwtnWTjHKfVOi7D7If0LQ83fpFrrftRQ/edit


I believe it maybe just a file i got spelled wrong but i am checking on that...

[Djur]

This is our team's custom mecanum code:


jaguarDrive((jX+jY)/2, (jY-jX)/2, -(jY-jX)/2, -(jX+jY)/2);


jX and jY are just the X and Y coordinates our joysticks read (-1 through 1).

And the jaguarDrive method:

public void jaguarDrive(double FL, double RL, double FR, double RR)
{
m_frontLeftMotor.set(FL);
m_rearLeftMotor.set(RL);
m_frontRightMotor.set(FR);
m_rearRightMotor.set(RR);
}


This is by far the simplest solution I've seen.

[Ether]

This is by far the simplest solution I've seen.

It's simple because you're only commanding 2 degrees of freedom.

[Djur]

It's simple because you're only commanding 2 degrees of freedom.




What other degrees of freedom are there, though? Turning is simple enough and isn't the topic of this thread anyhow.

[Ether]

What other degrees of freedom are there, though? Turning is simple enough and isn't the topic of this thread anyhow.

The code you posted commands only translation (fwd/rev & left/right), no rotation.

The topic of this thread is "help with mecanum drive code", so yes turning is part of mecanum drive code.

Adding rotation to the mix is not difficult. Simple code for that has been posted elsewhere.

[Djur]

The code you posted commands only translation (fwd/rev & left/right), no rotation.

The topic of this thread is "help with mecanum drive code", so yes turning is part of mecanum drive code.




Ah. Then I should just add that by setting FL, RL, FR, RR to the same value will make the robot turn. Positive values turn the robot counterclockwise, negative values turn it clockwise (assuming the motors are hooked up correctly).

Very simple example use for turning:

boolean turnRightAtFullSpeed = true;
if(turnRightAtFullSpeed)
jaguarDrive(-1, -1, -1, -1);

[Ether]

Ah. Then I should just add that by setting FL, RL, FR, RR to the same value will make the robot turn. Positive values turn the robot clockwise, negative values turn it counterclockwise (assuming the motors are hooked up correctly).

Very simple example use for turning:

boolean turnRightAtFullSpeed = true;
if(turnRightAtFullSpeed)
jaguarDrive(1, 1, 1, 1);


If you want full 3 degree of freedom control (rotate while translating), you should either use the provided WPI library codes for mecanum, or do something like this:

"y" is the forward command

"x" is the strafe right command

"z" is the rotate clockwise command


Step1:

Compute the 4 wheel speeds:

FL = y + x + z

FR = y - x - z

RL = y - x + z

RR = y + x - z


Step2:

Find the max absolute value of the above four commands,
and if it is greater than 1,
divide all four commands by that max absolute value.

[Djur]

"y" is the forward command

"x" is the strafe right command

"z" is the rotate clockwise command


Step1:

Compute the 4 wheel speeds:

FL = y + x + z

FR = y - x - z

RL = y - x + z

RR = y + x - z


Step2:

Find the max absolute value of the above four commands,
and if it is greater than 1,
divide all four commands by that max absolute value.




That's very interesting, although I see no point at which a robot would need to do that except for showing off.

[Ether]

This is our team's custom mecanum code:


jaguarDrive((jX+jY)/2, (jY-jX)/2, -(jY-jX)/2, -(jX+jY)/2);


jX and jY are just the X and Y coordinates our joysticks read (-1 through 1).


This is by far the simplest solution I've seen.

Simplicity comes at a price:

If jX is zero, your forward command to the wheels is limited to +/-0.5.

If jY is zero, your strafe command to the wheels is limited to +/-0.5

Only when |jX|=|jY|=1 will you ever be commanding full power.


That's very interesting, although I see no point at which a robot would need to do that except for showing off.

Most folks like to be able to turn while driving, rather than have to stop and rotate-in-place.

[austin1743]

well we have tried out code and it works but wants to go to the left the entire time as if it is in constant strafing mode. Also we have the joystick set-up in the code so when it goes forward to go forward but it goes backwards when we press forward.

I have tried changing the drive to being negative but this doesn't change anything. any suggestions?

[Ether]

well we have tried out code and it works but wants to go to the left the entire time as if it is in constant strafing mode. Also we have the joystick set-up in the code so when it goes forward to go forward but it goes backwards when we press forward.

I have tried changing the drive to being negative but this doesn't change anything. any suggestions?

Do you have the mec wheels mounted properly*? If so, continue:

Put the robot up on blocks.

Make an empty table on a clean sheet of paper and fill it in as you run these tests:

Give a pure forward command, and for each wheel record the direction it is spinning.

Give a pure reverse command, and for each wheel record the direction it is spinning.

Give a pure strafe_right command, and for each wheel record the direction it is spinning.

Give a pure strafe_left command, and for each wheel record the direction it is spinning.

Give a pure rotate_right command, and for each wheel record the direction it is spinning.

Give a pure rotate_left command, and for each wheel record the direction it is spinning.

Post the test results here.


* viewed from the top, rollers should form an "X" pattern, not an "O".

[austin1743]

yes we have the x made by the mecanum wheels. i will post the results up tomorrow when i go back in, thanks.

also in the code itself we have leftFrontspeed, rightFrontspeed do we need the back wheels set-up in the class the same way?