We Keep Coding

Robot Turning Using PID

I currently have a PID algorithm to control my robots turns in an autonomous state. My robot has encoders, on each motor, which there are four of, and also a BNO055IMU. Furthermore each motor is a never rest 40 motor from Andymark, and unfortunately I am stuck with encoders that do 3 pulses. I would like to improve the accuracy of my turns either by using a different algorithm or improving my current one.
My Current Turning Code:
public void turn(int angle, Direction DIRECTION, double timeOut, int sleepTime, double kp, double ki, double kd) {
double targetAngle = imu.adjustAngle(imu.getHeading() + (DIRECTION.value * angle));
double acceptableError = 0.5;
double currentError = 1;
double prevError = 0;
double integral = 0;
double newPower;
double previousTime = 0;
timeoutClock.reset();
while (opModeIsActive() && (imu.adjustAngle(Math.abs(currentError)) > acceptableError)
&& !timeoutClock.elapsedTime(timeOut, MasqClock.Resolution.SECONDS)) {
double tChange = System.nanoTime() - previousTime;
previousTime = System.nanoTime();
tChange = tChange / 1e9;
double imuVAL = imu.getHeading();
currentError = imu.adjustAngle(targetAngle - imuVAL);
integral += currentError * ID;
double errorkp = currentError * kp;
double integralki = integral * ki * tChange;
double dervitive = (currentError - prevError) / tChange;
double dervitivekd = dervitive * kd;
newPower = (errorkp + integralki + dervitivekd);
newPower *= color;
if (Math.abs(newPower) > 1.0) {newPower /= newPower;}
driveTrain.setPower(newPower, -newPower);
prevError = currentError;
DashBoard.getDash().create("TargetAngle", targetAngle);
DashBoard.getDash().create("Heading", imuVAL);
DashBoard.getDash().create("AngleLeftToCover", currentError);
DashBoard.getDash().update();
}
driveTrain.setPower(0,0);
sleep(sleepTime);
}
NOTES:
when driveTrain.setPower(x,y); is called the left parameter is the power set to the left side and the right parameter sets the right side.
Direction is an enum that stores wither -1, or 1 to switch between left and right turns.
Dashboard.getDash.create is solely to keep a log on what is going on.
imu.adjustAngle does the following:
public double adjustAngle(double angle) {
while (angle > 180) angle -= 360;
while (angle <= -180) angle += 360;
return angle;
}
imu.getHeading() is self explanatory it gets the yaw of the robot.
My current values for pid constants. (They work pretty well.)
KP_TURN = 0.005,
KI_TURN = 0.0002,
KD_TURN = 0,
ID = 1;

Calculations of the path of the Sun

I'm writing several methods necessary to calculate the path of the Sun across a specific point. I have written the code using two different sources for my calculations and neither is producing the desired result. The sources are: http://www.pveducation.org/pvcdrom/properties-of-sunlight/suns-position and
http://www.esrl.noaa.gov/gmd/grad/solcalc/solareqns.PDF
Note: Degrees to arcminutes is Deg * 60 min.
localSolartime: I have converted the longitude to 'minutes', the local standard time meridian(lstm) derived from the localStandardTimeMeridian method returns a value that is in 'minutes', and the equationOfTime which is also returned in 'minutes'. Using the equation from pveducation, I've calculated the time correction which accounts for the small time variations within a given time zone. When I apply this result and the localTime, each in minutes, to the local solar time (lst) equation, the result is 676.515 (at this moment), which does not make any sense to me. The local solar time, as I understand it, represents the time with respect to the Sun and when it is at its highest point in the sky, locally, is considered solar noon. 676.515 does not make sense. Does anybody understand what might be causing this.
HourAngle: I'm hoping that once I fix the localSolarTime method, this will not need to be corrected.
I've chosen Washington DC for the latitude and longitude. Both the Zenith and Azimuth readings should be positive values, and for my region at this moment, are 66 and 201 respectively.
public class PathOfSun {
static LocalTime localTime = LocalTime.now();
static double dcLat = 38.83;
static double dcLong = -77.02;
static DecimalFormat df = new DecimalFormat("#.0");
public static void main(String [] args) {
int day = dayOfYear();
double equationOfTime = equationOfTime(day);
double lstm = localTimeMeridian();
double lst = localSolarTime(equationOfTime, dcLong, lstm);
double declination = declination(day);
double hourAngle = hourAngle(lst);
double zenith = zenith(dcLat, declination, hourAngle);
double azimuth = azimuth(dcLong, declination, zenith, hourAngle);
}
//Longitude of timezone meridian
public static double localTimeMeridian() {
TimeZone gmt = TimeZone.getTimeZone("GMT");
TimeZone est = TimeZone.getTimeZone("EST");
int td = gmt.getRawOffset() - est.getRawOffset();
double localStandardTimeMeridian = 15 * (td/(1000*60*60)); //convert td to hours
//System.out.println("Local Time Meridian: " + localStandardTimeMeridian);
return localStandardTimeMeridian;
}
//Get the number of days since Jan. 1
public static int dayOfYear() {
Calendar localCalendar = Calendar.getInstance(TimeZone.getDefault());
int dayOfYear = localCalendar.get(Calendar.DAY_OF_YEAR);
//System.out.println("Day: " + dayOfYear);
return dayOfYear;
}
//Emperical equation to correct the eccentricity of Earth's orbit and axial tilt
public static double equationOfTime (double day) {
double d =(360.0/365.0)*(day - 81);
d = Math.toRadians(d);
double equationTime = 9.87*sin(2*d)-7.53*cos(d)-1.54*sin(d);
//System.out.println("Equation Of Time: " + equationTime);
return equationTime;
}
//The angle between the equator and a line drawn from the center of the Sun(degrees)
public static double declination(int dayOfYear) {
double declination = 23.5*sin((Math.toRadians(360.0/365.0))*(dayOfYear - 81));
//System.out.println("Declination: " + df.format(declination));
return declination;
}
//Add the number of minutes past midnight localtime//
public static double hourAngle(double localSolarTime) {
double hourAngle = 15 * (localSolarTime - 13);
System.out.println("Hour Angle: " + df.format(hourAngle)); //(degrees)
return hourAngle;
}
//Account for the variation within timezone - increases accuracy
public static double localSolarTime(double equationOfTime, double longitude, double lstm) {
//LocalSolarTime = 4min * (longitude + localStandardTimeMeridian) + equationOfTime
//Time Correction is time variation within given time zone (minutes)
//longitude = longitude/60; //convert degrees to arcminutes
double localStandardTimeMeridian = lstm;
double timeCorrection = (4 * (longitude + localStandardTimeMeridian) + equationOfTime);
System.out.println("Time Correction: " + timeCorrection); //(in minutes)
//localSolarTime represents solar time where noon represents sun's is highest position
// in sky and the hour angle is 0 -- hour angle is negative in morning, and positive after solar noon.
double localSolarTime = (localTime.toSecondOfDay() + (timeCorrection*60)); //(seconds)
localSolarTime = localSolarTime/(60*60); //convert from seconds to hours
//Convert double to Time (HH:mm:ss) for console output
int hours = (int) Math.floor(localSolarTime);
int minutes = (int) ((localSolarTime - hours) * 60);
//-1 for the daylight savings
Time solarTime = new Time((hours-1), minutes, 0);
System.out.println("Local Solar Time: " + solarTime); //hours
return localSolarTime;
}
public static double azimuth(double lat, double declination, double zenith, double hourAngle) {
double azimuthDegree = 0;
double elevation = 90 - zenith;
elevation = Math.toRadians(elevation);
zenith = Math.toRadians(zenith);
lat = Math.toRadians(lat);
declination = Math.toRadians(declination);
hourAngle = Math.round(hourAngle);
hourAngle = Math.toRadians(hourAngle);
//double azimuthRadian = -sin(hourAngle)*cos(declination) / cos(elevation);
double azimuthRadian = ((sin(declination)*cos(lat)) - (cos(hourAngle)*cos(declination)*
sin(lat)))/cos(elevation);
//Account for time quadrants
Calendar cal = Calendar.getInstance();
int hour = cal.get(Calendar.HOUR_OF_DAY);
if(hour > 0 && hour < 6) {
azimuthDegree = Math.toDegrees(acos(azimuthRadian));
}
else if(hour >= 6 && hour < 12) {
azimuthDegree = Math.toDegrees(acos(azimuthRadian));
azimuthDegree = 180 - azimuthDegree;
} else if (hour >= 12 && hour < 18) {
azimuthDegree = Math.toDegrees(acos(azimuthRadian));
azimuthDegree = azimuthDegree - 180;
} else if (hour >= 18 && hour < 24) {
azimuthDegree = Math.toDegrees(acos(azimuthRadian));
azimuthDegree = 360 - azimuthDegree;
}
System.out.println("Azimuth: " + df.format(azimuthDegree));
return azimuthDegree;
}
public static double zenith(double lat, double declination, double hourAngle) {
lat = Math.toRadians(lat);
declination = Math.toRadians(declination);
hourAngle = Math.round(hourAngle);
hourAngle = Math.toRadians(hourAngle);
//Solar Zenith Angle
double zenith = Math.toDegrees(acos(sin(lat)*sin(declination) + (cos(lat)*cos(declination)*cos(hourAngle))));
//Solar Elevation Angle
double elevation = Math.toDegrees(asin(sin(lat)*sin(declination) + (cos(lat)*cos(declination)*cos(hourAngle))));
System.out.println("Elevation: " + df.format(elevation));
System.out.println("Zenith: " + df.format(zenith));
return zenith;
}
}
Just to reiterate, the day, local time meridian are exactly correct, and the equation of time and declination are accurate but not exact.
----UPDATE OUTPUT----
-----UPDATE-----
Used the scatterchart to display the sun's elevation/azimuth throughout day. I am still having trouble figuring out the azimuth output. It is correct for long time, but then it will change from increasing and start to decrease (~270-->0). I will be sure to update the code once I finally get the output right.

You pass the longitude to localSolarTime() as degrees, and then you divide that by 60, with a comment claiming this is in order to convert to minutes of arc. This is wrong; your later calculations require degrees, and even if you needed minutes of arc, you'd multiply by 60, not divide.
This mistaken division results in a longitude of -1.3°, and when you find the angle between your local time meridian and your position, you get a large angle (about 75°). It should be a small angle, generally ±7.5°. The large angle results in a large time correction, and throws everything off.
Update: In the updated version of the azimuth() method, the quadrant selection should be based on the hour angle of the sun, or, equivalently, on local solar time, rather than standard wall clock time. And, the hour angle used in all calculations should not be rounded. Rather than testing four different quadrants, the method could look like this:
public static double azimuth(double lat, double declination, double zenith, double hourAngle)
{
double elevation = Math.toRadians(90 - zenith);
lat = Math.toRadians(lat);
declination = Math.toRadians(declination);
hourAngle = Math.toRadians(hourAngle);
double azimuthRadian = acos(((sin(declination) * cos(lat)) - (cos(hourAngle) * cos(declination) * sin(lat))) / cos(elevation));
double azimuthDegree = Math.toDegrees(azimuthRadian);
if (hourAngle > 0)
azimuthDegree = 360 - azimuthDegree;
System.out.println("Azimuth: " + df.format(azimuthDegree));
return azimuthDegree;
}
Finally, you are passing dcLong in as the lat parameter of the azimuth() method; this should be dcLat.
I'd recommend using radians internally throughout, and only converting from and to degrees on input and output. This will help prevent mistakes, and cut down on rounding errors and unnecessary clutter.

How to get direction of gravity

I need to calculate the linear acceleration based on the accelerometer, gyroscope and magnetometer. I found an application for android, which does exactly what I want to achieve:
https://play.google.com/store/apps/details?id=com.kircherelectronics.fusedlinearacceleration.
https://github.com/KEOpenSource/FusedLinearAcceleration
I'm trying to port it to a pure java. Because some elements of the code are based on virtual sensors (Gravity Sensor), I would like to achieve the same result by compute direction of gravity based on three basic sensors. I read that the force of gravity can be calculated using the Low Pass Filter (same as Android < 4.0), but this method does not give very accurate results.
From android 4.0, the force of gravity on each axis is calculated using sensor fusion. I found the code responsible for these measurements, but it is written in the CPP:
https://github.com/android/platform_frameworks_base/blob/ics-mr1/services/sensorservice/GravitySensor.cpp
Method used there is called "getRotationMatrix". The same method in SensorManager.java class: https://gitorious.org/android-eeepc/base/source/9cb3e09ec49351401cf19b5ae5092dd9ca90a538:core/java/android/hardware/SensorManager.java#L1034
public static boolean getRotationMatrix(float[] R, float[] I,
float[] gravity, float[] geomagnetic) {
// TODO: move this to native code for efficiency
float Ax = gravity[0];
float Ay = gravity[1];
float Az = gravity[2];
final float Ex = geomagnetic[0];
final float Ey = geomagnetic[1];
final float Ez = geomagnetic[2];
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
if (normH < 0.1f) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
return false;
}
final float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
final float Mx = Ay*Hz - Az*Hy;
final float My = Az*Hx - Ax*Hz;
final float Mz = Ax*Hy - Ay*Hx;
if (R != null) {
if (R.length == 9) {
R[0] = Hx; R[1] = Hy; R[2] = Hz;
R[3] = Mx; R[4] = My; R[5] = Mz;
R[6] = Ax; R[7] = Ay; R[8] = Az;
} else if (R.length == 16) {
R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = 0;
R[4] = Mx; R[5] = My; R[6] = Mz; R[7] = 0;
R[8] = Ax; R[9] = Ay; R[10] = Az; R[11] = 0;
R[12] = 0; R[13] = 0; R[14] = 0; R[15] = 1;
}
}
if (I != null) {
// compute the inclination matrix by projecting the geomagnetic
// vector onto the Z (gravity) and X (horizontal component
// of geomagnetic vector) axes.
final float invE = 1.0f / (float)Math.sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
final float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
final float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
if (I.length == 9) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[3] = 0; I[4] = c; I[5] = s;
I[6] = 0; I[7] =-s; I[8] = c;
} else if (I.length == 16) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[4] = 0; I[5] = c; I[6] = s;
I[8] = 0; I[9] =-s; I[10]= c;
I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
I[15] = 1;
}
}
return true;
}
takes four arguments:
float [] R, float [] I, float [] gravity, float [] Geomagnetic.
One of them is just gravity... The code I'm working on currently is similar to
https://github.com/KEOpenSource/FusedLinearAcceleration/blob/master/FusedLinearAcceleration/src/com/kircherelectronics/fusedlinearacceleration/sensor/LinearAccelerationSensor.java,
with the exception of methods that refer to SensorManager. These are copied from android source:
https://gitorious.org/android-eeepc/base/source/9cb3e09ec49351401cf19b5ae5092dd9ca90a538:core/java/android/hardware/SensorManager.java.
I did not found any examples of how implement this in Java.
So my question is: How I can implement method (in java), based only on three basic sensors, which returns me array of gravity direction (x, y, z), similar to Android one, but without using Android API.

Gravity is a steady contribution in the accelerometer signals (x, y & z).
So, logically, to isolate the gravity values in function of time, just low-pass filter the 3 accelerometer signals, at a frequency of 2Hz, for example.
A simple FIR would do the job.
On this site
I calculated the following coefficients:
[0.000381, 0.001237, 0.002634, 0.004607, 0.007100, 0.009956, 0.012928,
0.015711, 0.017987, 0.019480, 0.020000, 0.019480, 0.017987, 0.015711,
0.012928, 0.009956, 0.007100, 0.004607, 0.002634, 0.001237, 0.000381]
based on those caracteristics:
Fa=0Hz, Fb=1Hz, Length=21Pts, Fs=100Hz, Att=60dB.
You will get a signal that will be the three values of gravity in function of time.
You can find here some FIR explaination and Java implementation.

What you want is the rotation matrix (SensorManager.getRotationMatrix). Its last three components (i.e. rotation[6], rotation[7], rotation[8]) are the vector that points straight up, thus the direction to the center of the earth is the negative of that. To subtract gravity from your accelerometer reading just multiply that vector by g (~9.8m/s^2, though you might want to know that more precisely).

Calculating circular rotation required to match new angle

I have a data values which vary from +PI to -PI radian.
I need to get minimum rotation (in radians) required to go from old value to new value like:
float rotationRequired(float oldValue, float newValue){
return newValue - oldValue;
}
but simply subtracting won't do, because to go from -179 degree to +179 degree one does not need to rotate full circle around, just 2 degree clockwise. Because -PI = +PI in a circle are technically same rotation. Also the values can be in any range, i.e 740 = 360 + 360 + 20, hence only 20.
I'm thinking about breaking values into sin and cos values, subtract and then atan :
double oldY = Math.sin(oldValue);
double oldX = Math.cos(oldValue);
double newY = Math.sin(newValue);
double newX = Math.cos(newValue);
float delta = (float) Math.atan2( (newY - oldY),(newX - oldX) );
But still its not giving correct results, can anyone suggest another method ?

Just do the subtraction, and then limit the result to +/-180 by adding or subtracting 360 as necessary (the % operator may help here...)

I converted angles to degrees and used this method to suggest what minimum rotation is required and in what direction:
public static int suggestRotation(int o, int n){
//--convert to +0 to +360 range--
o = normalize(o);
n = normalize(n);
//-- required angle change --
int d1 = n - o;
//---other (360 - abs d1 ) angle change in reverse (opp to d1) direction--
int d2 = d1 == 0 ? 0 : Math.abs(360 - Math.abs(d1))*(d1/Math.abs(d1))*-1;
//--give whichever has minimum rotation--
if(Math.abs(d1) < Math.abs(d2)){
return d1;
}else {
return d2;
}
}
private static int normalize(int i){
//--find effective angle--
int d = Math.abs(i) % 360;
if(i < 0){
//--return positive equivalent--
return 360 - d;
}else {
return d;
}
}

Draw string on ellipse

I'm happy with the output of my clock, however - I am not sure how to properly align my drawString for the numbers which should appear at the tip of each tick mark on the clock.
I am hoping someone might be able show the proper method/formula for this.
private void drawTickMarks(Graphics2D g2)
{
double radius = this.faceRadius;
for (int secs = 0; secs <= 60; secs++)
{
double tickStart;
if (secs % 5 == 0)
tickStart = radius - 15;
else
tickStart = radius - 5;
tick = drawRadii(secs / 60.0, tickStart, radius);
if (secs % 5 == 0)
{
g2.setStroke(new BasicStroke(3));
g2.drawString(""+(secs/5),(int)tick.getX1()+(int)(tick.getX1()-tick.getX2()),
(int)tick.getY1()+(int)(tick.getY1()-tick.getY2()));
}
else
g2.setStroke(new BasicStroke(1));
g2.setColor(Color.WHITE);
g2.draw(tick);
}
}
Thanks to Thinhbk for a valid and correct solution with code and Jon W for the proper steps of coming to a solution.

If you imagine the String enclosed within a box, the x and y values you pass into drawString specify the lower left corner of the box.
I would modify the drawString line as such:
String number = ""+(secs/5);
int xLocation = (int)tick.getX1()+(int)(tick.getX1()-tick.getX2());
int yLocation = (int)tick.getY1()+(int)(tick.getY1()-tick.getY2());
int offsetX = /*Insert some value here to shift the position of all the strings
along the x-axis. Make this an expression that contains number.length(),
so that two-digit numbers are shifted more than one digit numbers. */
int offsetY = /*Insert some value here to shift the position of all the strings along
the y-axis.*/
g2.drawString(number, xLocation + offsetX, yLocation + offsetY);
You'll have to play around with the specific values for offsetX and offsetY to make it look nice.
If you want to be even fancier and make it so that drawString will automatically adjust the location depending on what font is being used, take a look at this and the FontMetrics class. You'll want to make offsetX and offsetY vary depending on width and height of the characters being drawn and whatnot.

As a supplemental to the solution provided by Jon W, I create a method that calculate the offset, and IMO, it looks fine. (#Jon W: sorry for not adding comment to your solution as it's rather long.)
/**
* Calculate the offset *
* #param i
* #return array:
* 0: x offset
* 1: y offset
*/
private int[] calculateOffSet(int i) {
int[] val = new int[2];
int deflt = -12;
if(i == 12) {
val[0] = -15;
val[1] = 9;
}else if (i > 6) {
val[0] = deflt + i - 6 ;
val[1] = i ;
}else {
val[0] = deflt + i ;
val[1] = i + 6;
}
return val;
}
And in your code, just call this:
int xLocation = (int)tick.getX1()+(int)(tick.getX1()-tick.getX2());
int yLocation = (int)tick.getY1()+(int)(tick.getY1()-tick.getY2());
int[] offset = calculateOffSet((secs / 5));
g2.drawString(number, xLocation + offset[0], yLocation + offset[1]);