Go to Post The magic is ALL in the mentors. - AdamHeard [more]
Home
Go Back   Chief Delphi > Technical > Programming
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 02-12-2018, 05:31 PM
RobotOrNoBot RobotOrNoBot is offline
Registered User
FRC #6322
 
Join Date: Feb 2018
Location: Florida
Posts: 4
RobotOrNoBot is an unknown quantity at this point
Question Autonomous Gyro Programming

I am trying to get our robot to read the field management system, and based on the color of the scale, drive to a certain angle and keep that angle/heading based on based on gyro information. I have the gyro working properly and it drives "straight" aka 0 degrees and corrects just fine. Also reading the field management system works great. But I cant seem to figure out how to make it drive to a set degree, like 20. I originally tried this by just adding said value to the gyro.getangle command. Obviously it did not work and the robot just spun in circles. We code in Java, and the gyro we use is the analog devices one that plugs into the SPI port on the robo rio. So I need to be able to specify an angle measurement and have the robot move from the calibration (zero degrees) and point itself at the specified angle, then go forward for a set amount of time, lastly stopping the motors. Any help is appreciated, and some example code wouldn't hurt either.

Code:
 public void autonomousPeriodic() {
		double threeright = gyro.getAngle(); 
		double tworight = gyro.getAngle();
		double twoleft = gyro.getAngle();
		double oneleft = gyro.getAngle();
		String gameData;
		gameData = DriverStation.getInstance().getGameSpecificMessage();
		if (m_timer.get() < 15) {
			if(gameData.length() > 0)
            {
	  if(gameData.charAt(0) == 'L')
	  {
		m_robotDrive.arcadeDrive(.6, twoleft*Kp);
	  } else {
		m_robotDrive.arcadeDrive(.6, tworight*Kp);
	  }
            }

		} else {
			m_robotDrive.stopMotor();// stop robot
		}
		}
Reply With Quote
  #2   Spotlight this post!  
Unread 02-12-2018, 05:44 PM
Oblarg Oblarg is offline
Registered User
AKA: Eli Barnett
FRC #0449 (The Blair Robot Project)
Team Role: Mentor
 
Join Date: Mar 2009
Rookie Year: 2008
Location: Philadelphia, PA
Posts: 1,804
Oblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond repute
Re: Autonomous Gyro Programming

Why not just use WPILib's PIDController (or one of its convenience wrappers, PIDSubsystem or PIDCommand)? No need to reinvent the wheel.
__________________
"Mmmmm, chain grease and aluminum shavings..."
"The breakfast of champions!"

Member, FRC Team 449: 2007-2010
Drive Mechanics Lead, FRC Team 449: 2009-2010
Alumnus/Technical Mentor, FRC Team 449: 2010-Present
Lead Technical Mentor, FRC Team 4464: 2012-2015
Technical Mentor, FRC Team 5830: 2015-2016


FRC Drivetrain Characterization
Reply With Quote
  #3   Spotlight this post!  
Unread 02-12-2018, 05:52 PM
connor.worley's Avatar
connor.worley connor.worley is offline
Lead Mentor
FRC #5499 (The Bay Orangutans)
 
Join Date: Mar 2011
Rookie Year: 2010
Location: Berkeley
Posts: 703
connor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond reputeconnor.worley has a reputation beyond repute
Re: Autonomous Gyro Programming

Hi,
Please read PID Without A PhD. Make sure you understand what your process variable (current sensor value), setpoint (desired sensor value), and error are as well as how they interact.
Then, print out these variables in real time and take a look at what's going on. Is your error growing instead of shrinking? Maybe you need to invert your output.
__________________
Team 5499 (2015-present)
Team 973 (2016-2017)
Team 254 (2014-2015)

Team 1538 (2011-2014)

日本語副専攻です!話しましょう!

Last edited by connor.worley : 02-12-2018 at 05:54 PM.
Reply With Quote
  #4   Spotlight this post!  
Unread 02-12-2018, 06:03 PM
gerthworm's Avatar
gerthworm gerthworm is offline
Making the 1's and 0's
FRC #1736 (Robot Casserole)
Team Role: Mentor
 
Join Date: Jan 2015
Rookie Year: 2015
Location: Peoria, IL
Posts: 331
gerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond repute
Re: Autonomous Gyro Programming

As posted, you'll need some sort of controller. A PID controller is an example of this.

You've identified the major components of the setup:

Desired Angle (ex: 20deg) - this is the direction you want to turn to

Actual Angle (gyro.getAngle()) - this is the direction you are actually going.

Output Device (drivetrain) - Assign some values to here to make Actual look more like Desired.


Usually, most controllers involve subtracting Actual and Desired (the resulting quantity is called the "error"). The controller's job is to assign outputs to drive the error to zero.

We use as simple proportional controller - at every loop, we read the gyro, and subtract it from the desired heading. This error is multipled by some constant value (Kp) which dictates how "aggressively" we correct for error.
Finally, the scaled error is added to one side of the drivetrain, subtracted from the other. Assuming you've got the signs right and Kp isn't too big or too small, this will push you back toward the angle you want to go at.

PID is this same concept, but with some extra tidbits thrown in.

WPIlib has a PID controller already implemneted for you. I suggest using it, but also to make sure you understand what the nature of the controller is before diving in head-first - this is one place where it really really helps to understand your goal before you start writing code.
Reply With Quote
  #5   Spotlight this post!  
Unread 02-12-2018, 06:41 PM
RobotOrNoBot RobotOrNoBot is offline
Registered User
FRC #6322
 
Join Date: Feb 2018
Location: Florida
Posts: 4
RobotOrNoBot is an unknown quantity at this point
Re: Autonomous Gyro Programming

Thanks for the help, can you guys elaborate more on how to use the PID controller specifically for the gyro? Ex. Do i need to instantiate anything?
Reply With Quote
  #6   Spotlight this post!  
Unread 02-12-2018, 08:02 PM
wlogeais wlogeais is offline
Registered User
FRC #2177 (The Robettes)
Team Role: Mentor
 
Join Date: Feb 2016
Rookie Year: 2011
Location: Minnesota
Posts: 60
wlogeais is an unknown quantity at this point
Re: Autonomous Gyro Programming

As a starting point you can create a sample-robot-project within your own ecplipse station

see; file > new > other > WPI examples > GearsBot.

Within that project you can look at ...robot.commands.DriveStraight.

This shows the instantiation and usage of the PIDControler class. That class can be copied to a new TurnChassis command, where setpoint and pid-get will be angles, and where the 'output' ... d -> Robot.m_drivetrain.drive(d, d));... can be changed to arcadeDrive(0,d).
Reply With Quote
  #7   Spotlight this post!  
Unread 02-12-2018, 08:10 PM
Jude99999 Jude99999 is offline
Registered User
FRC #6911 (SOBOTZ)
Team Role: Programmer
 
Join Date: Dec 2017
Rookie Year: 2018
Location: USA
Posts: 85
Jude99999 is an unknown quantity at this point
Re: Autonomous Gyro Programming

Quote:
Originally Posted by RobotOrNoBot View Post
Thanks for the help, can you guys elaborate more on how to use the PID controller specifically for the gyro? Ex. Do i need to instantiate anything?
I think that's a pretty good example on how to implement A PID with a Gyro. That should help you.

https://www.pdocs.kauailabs.com/navx...te-to-angle-2/
Reply With Quote
  #8   Spotlight this post!  
Unread 02-13-2018, 02:56 PM
RobotOrNoBot RobotOrNoBot is offline
Registered User
FRC #6322
 
Join Date: Feb 2018
Location: Florida
Posts: 4
RobotOrNoBot is an unknown quantity at this point
Re: Autonomous Gyro Programming

I tried to implement that example the best I could, but I have a feeling how I did it is very wrong. Any pointers?

Code:
public class Robot extends IterativeRobot{
	DifferentialDrive m_robotDrive = new DifferentialDrive(new Spark(0), new Spark(1));
	Joystick DriverStick = new Joystick(0);
	Joystick RampOpStick = new Joystick (1);
	Timer m_timer = new Timer();
	Compressor c = new Compressor(0);
    Solenoid rampLeft = new Solenoid(1);
	Solenoid rampRight = new Solenoid(2);
	Button button1 = new JoystickButton(DriverStick, 1);
	public static ADXRS450_Gyro gyro= new ADXRS450_Gyro();
	static final double kP = 0.3;
	static final double kI = 0.00;
	static final double kD = 0.00;
	static final double kF = 0.00;
	static final double kToleranceDegrees = 2.0f;
	PIDController turnController;
	  double rotateToAngleRate;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		CameraServer.getInstance().startAutomaticCapture(0);
		CameraServer.getInstance().startAutomaticCapture(1);
		gyro.calibrate();
		 turnController = new PIDController(kP, kI, kD, kF, gyro , (PIDOutput) this);
	      turnController.setInputRange(-180.0f,  180.0f);
	      turnController.setOutputRange(-1.0, 1.0);
	      turnController.setAbsoluteTolerance(kToleranceDegrees);
	      turnController.setContinuous(true);
	}

	/**
	 * This function is run once each time the robot enters autonomous mode.
	 */
	@Override
	public void autonomousInit() {
		m_timer.reset();
		m_timer.start();
	}

	/**
	 * This function is called periodically during autonomous.
	 */
	@Override
	public void autonomousPeriodic() {
		 boolean rotateToAngle = false;
		String gameData;
		gameData = DriverStation.getInstance().getGameSpecificMessage();
		if (m_timer.get() < 15) {
			if(gameData.length() > 0)
            {
	  if(gameData.charAt(0) == 'L')
	  {
		turnController.setSetpoint(29.6f);
		rotateToAngle = true;
	  } else {
		turnController.setSetpoint(12.6f);
		rotateToAngle = true;
	  }
            }

		} else {
			m_robotDrive.stopMotor();// stop robot
		}
		}
Reply With Quote
  #9   Spotlight this post!  
Unread 02-13-2018, 03:11 PM
Oblarg Oblarg is offline
Registered User
AKA: Eli Barnett
FRC #0449 (The Blair Robot Project)
Team Role: Mentor
 
Join Date: Mar 2009
Rookie Year: 2008
Location: Philadelphia, PA
Posts: 1,804
Oblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond reputeOblarg has a reputation beyond repute
Re: Autonomous Gyro Programming

Quote:
Originally Posted by RobotOrNoBot View Post
I tried to implement that example the best I could, but I have a feeling how I did it is very wrong. Any pointers?

Code:
public class Robot extends IterativeRobot{
	DifferentialDrive m_robotDrive = new DifferentialDrive(new Spark(0), new Spark(1));
	Joystick DriverStick = new Joystick(0);
	Joystick RampOpStick = new Joystick (1);
	Timer m_timer = new Timer();
	Compressor c = new Compressor(0);
    Solenoid rampLeft = new Solenoid(1);
	Solenoid rampRight = new Solenoid(2);
	Button button1 = new JoystickButton(DriverStick, 1);
	public static ADXRS450_Gyro gyro= new ADXRS450_Gyro();
	static final double kP = 0.3;
	static final double kI = 0.00;
	static final double kD = 0.00;
	static final double kF = 0.00;
	static final double kToleranceDegrees = 2.0f;
	PIDController turnController;
	  double rotateToAngleRate;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		CameraServer.getInstance().startAutomaticCapture(0);
		CameraServer.getInstance().startAutomaticCapture(1);
		gyro.calibrate();
		 turnController = new PIDController(kP, kI, kD, kF, gyro , (PIDOutput) this);
	      turnController.setInputRange(-180.0f,  180.0f);
	      turnController.setOutputRange(-1.0, 1.0);
	      turnController.setAbsoluteTolerance(kToleranceDegrees);
	      turnController.setContinuous(true);
	}

	/**
	 * This function is run once each time the robot enters autonomous mode.
	 */
	@Override
	public void autonomousInit() {
		m_timer.reset();
		m_timer.start();
	}

	/**
	 * This function is called periodically during autonomous.
	 */
	@Override
	public void autonomousPeriodic() {
		 boolean rotateToAngle = false;
		String gameData;
		gameData = DriverStation.getInstance().getGameSpecificMessage();
		if (m_timer.get() < 15) {
			if(gameData.length() > 0)
            {
	  if(gameData.charAt(0) == 'L')
	  {
		turnController.setSetpoint(29.6f);
		rotateToAngle = true;
	  } else {
		turnController.setSetpoint(12.6f);
		rotateToAngle = true;
	  }
            }

		} else {
			m_robotDrive.stopMotor();// stop robot
		}
		}
Casting Robot to PIDOutput is really strange, and is going to break if Robot doesn't implement the required methods.

Instead, pass the PIDController an empty anonymous inner class as a PIDOutput, and then use PIDController.get() to use the output synchronously from your main loop.
__________________
"Mmmmm, chain grease and aluminum shavings..."
"The breakfast of champions!"

Member, FRC Team 449: 2007-2010
Drive Mechanics Lead, FRC Team 449: 2009-2010
Alumnus/Technical Mentor, FRC Team 449: 2010-Present
Lead Technical Mentor, FRC Team 4464: 2012-2015
Technical Mentor, FRC Team 5830: 2015-2016


FRC Drivetrain Characterization
Reply With Quote
  #10   Spotlight this post!  
Unread 02-13-2018, 07:26 PM
Jude99999 Jude99999 is offline
Registered User
FRC #6911 (SOBOTZ)
Team Role: Programmer
 
Join Date: Dec 2017
Rookie Year: 2018
Location: USA
Posts: 85
Jude99999 is an unknown quantity at this point
Re: Autonomous Gyro Programming

Quote:
Originally Posted by RobotOrNoBot View Post
I tried to implement that example the best I could, but I have a feeling how I did it is very wrong. Any pointers?

Code:
public class Robot extends IterativeRobot{
	DifferentialDrive m_robotDrive = new DifferentialDrive(new Spark(0), new Spark(1));
	Joystick DriverStick = new Joystick(0);
	Joystick RampOpStick = new Joystick (1);
	Timer m_timer = new Timer();
	Compressor c = new Compressor(0);
    Solenoid rampLeft = new Solenoid(1);
	Solenoid rampRight = new Solenoid(2);
	Button button1 = new JoystickButton(DriverStick, 1);
	public static ADXRS450_Gyro gyro= new ADXRS450_Gyro();
	static final double kP = 0.3;
	static final double kI = 0.00;
	static final double kD = 0.00;
	static final double kF = 0.00;
	static final double kToleranceDegrees = 2.0f;
	PIDController turnController;
	  double rotateToAngleRate;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		CameraServer.getInstance().startAutomaticCapture(0);
		CameraServer.getInstance().startAutomaticCapture(1);
		gyro.calibrate();
		 turnController = new PIDController(kP, kI, kD, kF, gyro , (PIDOutput) this);
	      turnController.setInputRange(-180.0f,  180.0f);
	      turnController.setOutputRange(-1.0, 1.0);
	      turnController.setAbsoluteTolerance(kToleranceDegrees);
	      turnController.setContinuous(true);
	}

	/**
	 * This function is run once each time the robot enters autonomous mode.
	 */
	@Override
	public void autonomousInit() {
		m_timer.reset();
		m_timer.start();
	}

	/**
	 * This function is called periodically during autonomous.
	 */
	@Override
	public void autonomousPeriodic() {
		 boolean rotateToAngle = false;
		String gameData;
		gameData = DriverStation.getInstance().getGameSpecificMessage();
		if (m_timer.get() < 15) {
			if(gameData.length() > 0)
            {
	  if(gameData.charAt(0) == 'L')
	  {
		turnController.setSetpoint(29.6f);
		rotateToAngle = true;
	  } else {
		turnController.setSetpoint(12.6f);
		rotateToAngle = true;
	  }
            }

		} else {
			m_robotDrive.stopMotor();// stop robot
		}
		}
Code:
	GyroPid = new PIDController(kP, kI, kD, Robotmap.ahrs, this);
		GyroPid.setInputRange(-180.0, 180.0);
		GyroPid.setOutputRange(-0.7, 0.7);
		GyroPid.setAbsoluteTolerance(1.0f);
		GyroPid.setContinuous(true);

GyroPid.setSetpoint(0.0);

GyroPid.enable();


///You'll have to override this method in your class
	@Override
	public void pidWrite(double output) {
		// TODO Auto-generated method stub
	 			Driver.arcadeDrive(0.06, output);

	}
Try to implement this code and I think it should work. After you'll have to tune the PID gains (kP,kI,kD) terms.
Reply With Quote
  #11   Spotlight this post!  
Unread 02-13-2018, 08:49 PM
NewtonCrosby NewtonCrosby is offline
Registered User
FRC #1895 (LamdaCorps)
Team Role: Mentor
 
Join Date: Jan 2018
Rookie Year: 2012
Location: Virginia
Posts: 24
NewtonCrosby is an unknown quantity at this point
Re: Autonomous Gyro Programming

If you take a look at our code from last year, it was the first year we ever tried to do anything with PID controls.....so we know the pain you are dealing with in trying to figure it out.

First thing first, you can checkout our public repo at the link: https://github.com/Lambda-Corps/2017Steamworks_Public


Within that code base you will find two commands that are exactly what you want to do. We have both PID and non-PID versions of commands that turn to a certain angle, and commands that drive straight for a set distance.

You will have to calibrate this code for your own Drivetrain class, motor controllers etc (we use Talons), however you should be able to turn our code into your own functioning commands pretty easily.

The next thing you need to understand is how to tune the PID controllers to properly work for your own robot. Here is a youtube video that generally explains the process for you:
https://www.youtube.com/watch?v=yqD9iHiR3j8


Good luck,
Reply With Quote
  #12   Spotlight this post!  
Unread 02-16-2018, 02:59 PM
RobotOrNoBot RobotOrNoBot is offline
Registered User
FRC #6322
 
Join Date: Feb 2018
Location: Florida
Posts: 4
RobotOrNoBot is an unknown quantity at this point
Re: Autonomous Gyro Programming

Figured it out, it was as simple as subtracting the gyro's current value from the desired angle.
Reply With Quote
  #13   Spotlight this post!  
Unread 02-17-2018, 11:29 PM
garyk garyk is offline
Programming Mentor, 972 and 2643
FRC #2643 (Dark Matter)
Team Role: Mentor
 
Join Date: Dec 2006
Rookie Year: 2005
Location: Santa Clara (Silicon Valley) Calif.
Posts: 169
garyk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant futuregaryk has a brilliant future
Re: Autonomous Gyro Programming

There’s a Week 0 report https://www.chiefdelphi.com/forums/s...60&postcount=7 that the “gamedata” FMS field element color string is coming late. Week 0 competitions are used, among other things, to test FMS. Maybe it’ll be fixed, maybe it won’t.

Thanks to BenBernard 5687 for this Week 0 report.
__________________

Silicon Valley Regional 2005, 2006 972
Silicon Valley Regional 2007 668 Xerox Creativity Award
Championship Event 2007 668
Portland Regional 2008 668
Silicon Valley Regional 2008 668, 972
Beta Test Team 2008 668 (with 100 & 254)
Silicon Valley Regional 2009 668 Regional Chairman's Award; 2643
Sacramento Regional 2009 668 Winning Alliance (thanks, 1717 & 2473!), 2010 Winning Alliance 3256
CalGames 2006, 2007, 2008, 2009, 2010, 2011 Field Tech
NorCal FTC Regional 2008, 2009 Inspector
Championship Event 2009
San Diego, Silicon Valley Regionals; Champ. Event 2010 668, 2643, 3256
Silicon Valley, Madera Regional 2012 2643
WRRF Programming Instructor 2006-2016
Regional Woodie Flowers Award 2014 2643 Utah Regional

Reply With Quote
  #14   Spotlight this post!  
Unread Yesterday, 07:22 PM
BenBernard's Avatar
BenBernard BenBernard is offline
Programming and Controls Mentor
FRC #5687 (The Outliers)
Team Role: Mentor
 
Join Date: Jan 2016
Rookie Year: 2015
Location: Portland, ME
Posts: 106
BenBernard will become famous soon enoughBenBernard will become famous soon enough
Re: Autonomous Gyro Programming

Please note the correction later in that thread. I discovered in the logs that my original explanation was wrong.
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 12:27 AM.

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


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