Mecanum-Code

[FRC TEAM 4085]

This year we are trying to use mecanum wheels for the first time. However, we are having some programming issues when it comes to using different controllers. We are currently trying to find a code to work an Xbox 360 controller. Below is the current code that we are trying to use, but we can't seem to get it to work. If you have any advice on what we should try or do, please let us know. Also, we are a Java only team, as we have no other experience with other languages.

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. 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 edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import com.sun.sqawk.util.MathUtils;


//Mecanum class. We are using IterativeRobot as SimpleRobot was not working.


public class RobotTemplate extends IterativeRobot {
//Global Settings:
//PWN Positions
private static final int FRONT_LEFT_PWM = 1;
private static final int FRONT_RIGHT_PWM = 2;
private static final int REAR_LEFT_PWM = 4;
private static final int REAR_RIGHT_PWM = 3;

//Joystick Threshold
private static final double THRESHOLD = 0.2;


//Get the joysticks (keep joy* naming system so it can be adjusted later)
//Joystick joy1 = new Joystick(1);
Joystick joy2 = new Joystick(2);
Joystick joy3 = new Joystick(3);
Joystick mXboxController = new Joystick(1);
double leftX = mXboxController.getRawAxis(1);
double leftY = mXboxController.getRawAxis(2);
double rightX = mXboxController.getRawAxis(4);
double rightY = mXboxController.getRawAxis(5);
boolean trigger = mXboxController.getRawButton(6);
//Get the jaguars
Jaguar front_right = new Jaguar(FRONT_RIGHT_PWM);
Jaguar front_left = new Jaguar(FRONT_LEFT_PWM);
Jaguar rear_right = new Jaguar(REAR_RIGHT_PWM);
Jaguar rear_left = new Jaguar(REAR_LEFT_PWM);

//Setup RobotDrive
RobotDrive drive = new RobotDrive(front_right, front_left, rear_right, rear_left);


//Button button1L = new JoystickButton(joy1, 1);



//When robot starts
public void robotInit() {
//Invert only the right side motors. Keep the left side normal.

//Disable the Watchdog as it can cause issues with this version of java.

}

//When robot is disabled
public void disabledInit() {
//Stop the motors for safety
front_left.set(0);
rear_left.set(0);
front_right.set(0);
rear_right.set(0);
//Log disabled status

}

//Periodically called during autonomous
public void autonomousPeriodic() {

}

//When robot is started in autonomous
public void autonomousInit() {
//Set the iOS boolean to the opposite of digital input 1.

//Log initiation success

}

//Called at the start of teleop
public void teleopInit() {
//Log initiation success

}

//Periodically called during teleop
public void teleopPeriodic() {

if(mXboxController.getRawButton(6)){

drive.tankDrive(leftY, rightY);
Timer.delay(0.01);


}

else{

double X, Y, Z;
//Check if using iOS interface or not

{
//Standard controls
X = leftX;
Z = leftY;
Y = rightX;
}
//Threshold
if( Math.abs(X) < THRESHOLD ){
X = 0;
}
if( Math.abs(Y) < THRESHOLD ){
Y = 0;
}
if( Math.abs(Z) < THRESHOLD ){
Z = 0;
}
//Get the magnitude using the distance formula
double magnitude = Math.sqrt((X*X)+(Y*Y));
//Normalize the value
if( magnitude > 1 ){
magnitude = 1;
}

double Direction; //Defineing the direction
// Direction = Math.tan(X/Y); //Tan of pheta is equal to the ajacent over the opposite.
Direction = MathUtils.atan(X/Y); //Figured out how to do it added a Math(Utils).atan for it to find it!
//Drive
drive.mecanumDrive_Polar( magnitude, Direction, Z);



}
}

}

[Joe Ross]

What behavior do you see?

I noticed that the joysticks are only read once at the beginning of the program, rather then inside telopPeriodic.

[nyaculak]

Like Joe Ross said, your code only evaluates the value of the axes once, before teleoperated mode is ever started. To fix this, you need to retrieve the values of the axes you wish to use at the beginning of the teleop loop.

Also, for a mecanum drive robot, you want to call a mecanum drive function instead of the tankDrive() function. The RobotDrive defines two mecanum drive functions: mecanumDrive_Cartesian and mecanumDrive_Polar. Take a look at the Javadoc (http://team2168.org/javadoc/) to learn how to implement them.

[FRC TEAM 4085]

What behavior do you see?

I noticed that the joysticks are only read once at the beginning of the program, rather then inside telopPeriodic.

To Joe, The program is not recognizing any inputs at all from the xbox controller but are in fact recognizing from the attack 3 controller with the kit of parts.

[FRC TEAM 4085]

Like Joe Ross said, your code only evaluates the value of the axes once, before teleoperated mode is ever started. To fix this, you need to retrieve the values of the axes you wish to use at the beginning of the teleop loop.

Also, for a mecanum drive robot, you want to call a mecanum drive function instead of the tankDrive() function. The RobotDrive defines two mecanum drive functions: mecanumDrive_Cartesian and mecanumDrive_Polar. Take a look at the Javadoc (http://team2168.org/javadoc/) to learn how to implement them.

We added a fail safe system for if any reason the mecanum code doesn't work or the driver doesn't feel safe using the mecanum code we added tank drive which is a drive style our team feels very comfortable with while the mecanum code is new.

[Ether]

We added a fail safe system for if any reason the mecanum code doesn't work or the driver doesn't feel safe using the mecanum code we added tank drive which is a drive style our team feels very comfortable with while the mecanum code is new.

Food for thought, here's another option:

Joystick1 drives the vehicle in Arcade mode (Y fore/aft and X is rotate);

Joystick2 controls strafe (X axis controls strafe right/left).


Leave hands off Joystick2 and you've got basic Arcade drive.

[nyaculak]

We added a fail safe system for if any reason the mecanum code doesn't work or the driver doesn't feel safe using the mecanum code we added tank drive which is a drive style our team feels very comfortable with while the mecanum code is new.

Sorry, I should have looked at the rest of your teleop code.

To Joe, The program is not recognizing any inputs at all from the xbox controller but are in fact recognizing from the attack 3 controller with the kit of parts.


You might want to check that the drivers for the Xbox controller are installed (I've never had a problem with it; one would think the Microsoft hardware and software would work well together). You should be able to see the Xbox controller as a device somewhere in the Windows Control Panel. Does the driver station recognize it as a controller when you plug it in?

Also, you'll still want to move the retrieval of the axis values to the beginning of the loop.

Another tip: the tangent of an angle equals opposite over adjacent (y/x).

[BradAMiller]

A couple things that might be helpful for you:

This ChiefDelphi post about Mecanum drive and C++
http://www.chiefdelphi.com/forums/showthread.php?t=88686

and this ScreenSteps document that essentially extracted the sample program from the above CD post:
http://wpilib.screenstepslive.com/s/3120/m/7912/l/171875-driving-a-robot-using-mecanum-drive

[Joe Ross]

To Joe, The program is not recognizing any inputs at all from the xbox controller but are in fact recognizing from the attack 3 controller with the kit of parts.

I can't see how it's possible for the program you posted to recognize inputs from any joystick. Are you sure that is the program you were testing with the Attack 3?