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);
           
   
            
}
}

}