Go to Post It kicked off today, and we already have 10 pages of posts!?!?! - ShotgunNinja [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 13-04-2015, 22:39
ArzaanK ArzaanK is offline
Registered User
FRC #1325 (Inverse Paradox)
Team Role: Programmer
 
Join Date: Jan 2013
Rookie Year: 2012
Location: Mississauga, Ontario, Canada
Posts: 40
ArzaanK is an unknown quantity at this point
Programming SPI Gyro

Hello Chief Delphi,
Our team recently ordered new gyros. We decided to go with the EVAL-ADXRS453Z boards. (http://www.analog.com/en/products/me...duct-overview).

The gyroscopes run on the SPI bus, and do not use an analog input like the KOP gyros. We have never programmed using SPI before, and we were hoping we could get some help. We have looked through the WPILib class documentation, and searched for examples, but we are having a hard time finding any. Also, it seems like there are no built in SPI examples this year in Eclipse.

We have written some base initialization code such as,
Code:
        SPI spiGyro = new SPI(Port.kOnboardCS0) ;
    	spiGyro.setClockRate(4000000);
    	spiGyro.setClockActiveHigh();
    	spiGyro.setChipSelectActiveLow();
    	spiGyro.setMSBFirst();
However, we do not know where to go from here. From the class documentation, it seems like we could have to use a combination of the read, write and transaction methods in order to receive data from the sensor.

If any of you have used SPI in the past, have example code we could look at, or just help us out, that would be greatly appreciated.
__________________
Arzaan Khairulla
Programmer/Driver
2013 Greater Toronto Regional East Winners with 1114 and 2056
2013 Galileo Division
Reply With Quote
  #2   Spotlight this post!  
Unread 13-04-2015, 23:01
GeeTwo's Avatar
GeeTwo GeeTwo is online now
Technical Director
AKA: Gus Michel II
FRC #3946 (Tiger Robotics)
Team Role: Mentor
 
Join Date: Jan 2014
Rookie Year: 2013
Location: Slidell, LA
Posts: 3,687
GeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond repute
Re: Programming SPI Gyro

We considered doing this immediately following bag-and-tag when our next-to-last analog gyro died. We had hoped to extend the AnalogChannel class and get the integration done for us by using the Gyro(AnalogChannel) constructor. We learned that the integration was done in hardware, so we would have to do it manually. A gyroscope gives you a rotation rate-of-change, so the computer must integrate that rate to determine the actual orientation. At that point, we switched to encoders instead, and trusted that the wheels didn't slip. For more info, see this thread on usfirst. We may tackle this as an off-season project, or get one of those integrated solutions (e.g. the AndyMark NavX MXP board) that are out of stock for next year.
__________________

If you can't find time to do it right, how are you going to find time to do it over?
If you don't pass it on, it never happened.
Robots are great, but inspiration is the reason we're here.
Friends don't let friends use master links.

Last edited by GeeTwo : 13-04-2015 at 23:05.
Reply With Quote
  #3   Spotlight this post!  
Unread 14-04-2015, 01:02
slibert slibert is offline
Software Mentor
AKA: Scott Libert
FRC #2465 (Kauaibots)
Team Role: Mentor
 
Join Date: Oct 2011
Rookie Year: 2005
Location: Kauai, Hawaii
Posts: 356
slibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud ofslibert has much to be proud of
Re: Programming SPI Gyro

Quote:
Originally Posted by GeeTwo View Post
We considered doing this immediately following bag-and-tag when our next-to-last analog gyro died. We had hoped to extend the AnalogChannel class and get the integration done for us by using the Gyro(AnalogChannel) constructor. We learned that the integration was done in hardware, so we would have to do it manually. A gyroscope gives you a rotation rate-of-change, so the computer must integrate that rate to determine the actual orientation. At that point, we switched to encoders instead, and trusted that the wheels didn't slip. For more info, see this thread on usfirst. We may tackle this as an off-season project, or get one of those integrated solutions (e.g. the AndyMark NavX MXP board) that are out of stock for next year.
The navX MXP's are in stock now at both AndyMark and the Kauai Labs store, feel free to private message me if you have any questions.

On the navX MXP wiki in the Application Example section, there are several robot examples for features including Field-centric drive, auto-balancing, motion detection and auto-rotate to angle. Team 2465's mecanum drive featured field-centric drive and auto-rotate to angle based on the navX MXP and it worked very well - and you can find the open-source code for this here.

Last edited by slibert : 14-04-2015 at 02:12.
Reply With Quote
  #4   Spotlight this post!  
Unread 14-04-2015, 08:19
otherguy's Avatar
otherguy otherguy is offline
sparkE
AKA: James
FRC #2168 (The Aluminum Falcons)
Team Role: Mentor
 
Join Date: Feb 2010
Rookie Year: 2009
Location: CT
Posts: 434
otherguy is a splendid one to beholdotherguy is a splendid one to beholdotherguy is a splendid one to beholdotherguy is a splendid one to beholdotherguy is a splendid one to beholdotherguy is a splendid one to beholdotherguy is a splendid one to behold
Re: Programming SPI Gyro

We used the gyro you mentioned in the original post all season with good results. The other programming mentor on our team (kevin[at]team2168.org) spent time getting this class up and running.

Our source code to interface with this sensor is available here: https://gist.github.com/jcorcoran/6d721cf6570f772b51f8

Once the sensor is wired up, the code is pretty easy to use.

We instantiate it in our drivetrain susbsystem:
Code:
public ADXRS453Gyro gyroSPI;
gyroSPI = new ADXRS453Gyro();
gyroSPI.startThread();
You can call gyroSPI.getAngle(); to get the current heading relative to the position that the robot was at when it turned on.


There's also a feature built in which allows you to recalibrate the gyro. Calibration will occur automatically on startup. It's a 10 second calibration sequence, and should only be called when you expect the robot to be stationary for this duration of time (disabled before a match).

This feature is there to allow the robot to re-calibrate itself before a match if the robot was say, turned on while it was being placed on the field, or moved around while it was calibrating. This affects the static drift rate, so it will throw off all subsequent calculations. Our recalibration code looks something like this in our disabledPeriodic method:

Code:
//Check to see if the gyro is drifting, if it is re-initialize it.
//Thanks FRC254 for orig. idea.
curAngle = drivetrain.getHeading();
gyroCalibrating = drivetrain.isGyroCalibrating(); //this is a call into ADXRS453Gyro.isCalibrating();

if (lastGyroCalibrating && !gyroCalibrating) {
	//if we've just finished calibrating the gyro, reset
	gyroDriftDetector.reset();
	curAngle = drivetrain.getHeading();
	System.out.println("Finished auto-reinit gyro");
} else if (gyroDriftDetector.update(Math.abs(curAngle - lastAngle) > (0.75 / 50.0))
		&& !matchStarted && !gyroCalibrating) {
	//&& gyroReinits < 3) {
	gyroReinits++;
	System.out.println("!!! Sensed drift, about to auto-reinit gyro ("+ gyroReinits + ")");
	drivetrain.calibrateGyro();
}

lastAngle = curAngle;
lastGyroCalibrating = gyroCalibrating;

It's possible that a match will start while the gyro is calibrating. So if you're going to use the recalibration code, you should ensure that you add a call to ADXRS453Gyro.stopCalibrating() inside your autonomousInit() method, so that the gyro completes its calibration sequence prior to match start. For your information, if you call stopCalibrating, while a calibration sequence is active, it will fail gracefully with a partial calibrated zero drift value. See implementation at line 325 of ADXRS453Gyro.java.

In the above code gyroDriftDetector is an instance of the Debouncer class.

If you have any questions please shoot an e-mail to kevin[at]team2168.org or myself james[at]team2168.org
__________________
http://team2168.org

Last edited by otherguy : 14-04-2015 at 08:22.
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:07.

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