Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)
-   -   Programming the ADIS_16448 IMU for mecanum field oriented driving (http://www.chiefdelphi.com/forums/showthread.php?t=154685)

18gliccolt 03-02-2017 19:32

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



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

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