Go to Post Almost fell out of my chair in the CAD room once. - ChristopherSD [more]
Home
Go Back   Chief Delphi > Technical > Programming > Java
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 03-02-2017, 19:32
18gliccolt 18gliccolt is offline
Registered User
FRC #6164
 
Join Date: Feb 2017
Location: Iowa
Posts: 1
18gliccolt is an unknown quantity at this point
Programming the ADIS_16448 IMU for mecanum field oriented driving

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 ) 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:
Code:
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();
	}
}
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 13:15.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi