I’m one of the programmers on team 6164. This is my first time programming for FRC although I’ve done a bit of general programming before.
I want to get field oriented driving for our robot with mecanum wheels. The way I understand it is you pass the imu.getAngle() to mecanumDrive_Cartesian. I also think that the ADIS 16448 uses the magnetic field of the earth to get the angle value. What I want it to do is, when the robot starts set the angle to 0 and go from there. That way everything is relative to the angle the robot starts in. Since I couldn’t find anything to set the IMU to 0, I get the angle on startup and set that to a variable then subtract that from the new readings. Everything seems right mathematically (unless I’m missing something huge :P) but the bot is very jerky. Even if i give the mecanum drive method the angle straight from the IMU the bot is still jerky. If i set the angle a constant of 0 then it drives much smoother but its not field oriented which is what i want.
here is what i have for code so far:
package org.usfirst.frc.team6164.robot;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.TalonSRX;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import org.usfirst.frc.team6164.robot.commands.ExampleCommand;
import org.usfirst.frc.team6164.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj.PWM;
import com.ctre.CANTalon;
import com.analog.adis16448.frc.ADIS16448_IMU;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot {
public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
public static OI oi;
CANTalon BL = new CANTalon(1);
CANTalon BR= new CANTalon(2);
CANTalon FR= new CANTalon(3);
CANTalon FL= new CANTalon(4);
RobotDrive drive = new RobotDrive(FL,BL,FR,BR); //2 motor driver
Joystick stick = new Joystick(0);
ADIS16448_IMU imu = new ADIS16448_IMU();
double heading;
boolean setHeading = true;
Command autonomousCommand;
SendableChooser<Command> chooser = new SendableChooser<>();
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
oi = new OI();
chooser.addDefault("Default Auto", new ExampleCommand());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
FL.setVoltageRampRate(50);
FR.setVoltageRampRate(50);
FR.setInverted(true);
BL.setVoltageRampRate(50);
//BL.setInverted(true);
BR.setVoltageRampRate(50);
BR.setInverted(true);
}
/**
* 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() {
autonomousCommand = 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 (autonomousCommand != null)
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 (autonomousCommand != null)
autonomousCommand.cancel();
drive.setSafetyEnabled(false);
}
/**
* This function is called periodically during operator control
*/
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
if (setHeading){
heading = imu.getAngle();
setHeading = false;
}
System.out.println("percived heading"+ (imu.getAngle()-heading));
System.out.println("raw angle"+imu.getAngle());
System.out.println("heading "+heading);
drive.mecanumDrive_Cartesian(stick.getX()/2, stick.getY()/2, stick.getRawAxis(4)/2, imu.getAngle()-heading);//imu.getAngle()/2);
Timer.delay(.01);
}
/**
* This function is called periodically during test mode
*/
@Override
public void testPeriodic() {
LiveWindow.run();
}
}