We Keep Coding

Apply Low Pass Filter

How to apply this Low Pass Filter Function in my code:
static final float alpha = 0.05f;
public float[] lowPass(float[] output, float[] input)
{
output[0] = output[0] + alpha * (input[0] - output[0]);
output[1] = output[1] + alpha * (input[1] - output[1]);
output[2] = output[2] + alpha * (input[2] - output[2]);
return output;
}
Reading Accelerometer and detecting an accident
I Want mLastX, mLastY and mLastZ save accelerometer readings after being filtered with low pass filter
public void onSensorChanged(int sensor, float[] values)
{
if (sensor != SensorManager.SENSOR_ACCELEROMETER) return;
long now = System.currentTimeMillis();
if ((now - mLastForce) > SHAKE_TIMEOUT) {
mShakeCount = 0;
}
if ((now - mLastTime) > TIME_THRESHOLD) {
long diff = now - mLastTime;
float speed = Math.abs(values[SensorManager.DATA_X] + values[SensorManager.DATA_Y] + values[SensorManager.DATA_Z] - mLastX - mLastY - mLastZ) / diff * 10000;
if (speed > FORCE_THRESHOLD) {
if ((++mShakeCount >= SHAKE_COUNT) && (now - mLastShake > SHAKE_DURATION)) {
mLastShake = now;
mShakeCount = 0;
if (mShakeListener != null) {
mShakeListener.onShake();
}
}
mLastForce = now;
}
mLastTime = now;
mLastX = values[SensorManager.DATA_X];
mLastY = values[SensorManager.DATA_Y];
mLastZ = values[SensorManager.DATA_Z];

Call it with the first parameter initially being the array [mlastx, mlastY, mlastz] and the second parameter being the current values of the sensor. After being called, the values of the first array will be changed and should become mlastX, mlastY, and mlastZ. Its actually easier to code if you just keep them as an array rather than holding them in separate variables.
Now wether this will accomplish what you want is another question. Your math as it is in that function has some odd things in it, and a few things just outright wrong. For example, you're summing accelerations and dividing by time to get a "speed". No, that gets you jerk. Speed would require you to multiply by time. And to get a true vector magnitude you should be adding the squares of the speed differences for each component, not the absolute values. THe 10K multiplication makes no sense, just divide the threshold by 10K, as its unlikely to be used anywhere else. This has a feeling of someone who tried to translate an algorithm they didn't really understand.

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.

Libgdx - Slowing Projectile to stop, then Reversing it

When working with creating games in Libgdx, I have not been using the physics engine as I dont have too many moving parts in the game. So, to have objects fall from the top of the screen to the bottom, I used what the documentation had, something like this:
projectile.y -= 200 * Gdx.graphics.getDeltaTime();
That example will make said projectile go down at 200 pixels per second (I believe). What I am trying to do is make it so after two seconds, the projectile will transition from negative 200 per second, to positive 200 per second. I've tried using loops and Thread.sleep, but that will just freeze the entire game and unfreeze with the projectiles going the other way. Any ideas?

Linear interpolation.
All you need to do is determine the start point: x1 = -200
Determine the end point: x2 = 200
Determine the amount of seconds that it takes to reach the end point: tmax = 2.0 sec
Determine the difference that you need to add to the original to reach the end point: v = (x2-x1) = (200 - (-200)) = 400
Use the linear interpolation function: x1 + t*v = x2 where t e [0...1] //must be normalized to 0..1 interval
Thus at t = 0, the value is at x1 + 0 = x1; and at t = (tn/tmax) [which is 1], the value is at x1 + v = x2.
So all you need is a timer from 0 to 2 and the following equation:
float interpolationTimer = 0.0f;
final float interpolationTimerMax = 2.0f;
public void render()
{
float delta = Gdx.graphics.getDeltaTime();
interpolationTimer += delta;
if(interpolationTimer > interpolationTimerMax )
{
interpolationTimer = interpolationTimerMax ;
}
velocity.y = -200 + (interpolationTimer/interpolationTimerMax) * (400); //x1 + t*v = x2
projectile.y -= velocity.y * delta;
}

To get a change in direction of y, you need a polynomial function of x. For a single direction change, use a binomial; try something like
projectile.y = projectile.y
- 200 * Gdx.graphics.getDeltaTime()
+ 20 * Math.pow(Gdx.graphics.getDeltaTime(), 2);

If you are looking for linear interpolation of the speeds, just keep track of time.
float timeElapsed = 0.0f;
void render() {
timeElapsed += Gdx.graphics.getDeltaTime();
projectile.y -= 200.0f * (1.0f - timeElapsed);
}
Be sure to stop after timeElapsed has reached 2 seconds (that's if(timeElapsed < 2.0f)). Time elapsed variable will start at 0.0f and will slowly increment. Until it reaches 1.0f, projectile.y will get substracted from. However, as soon as time elapsed is higher than 1.0f, projectile.y will get added to.

Java performance optimization

For a JOGL game I get very low fps, now with some testing I found out the problem is not in the JOGL part, but in pure Java calculations. I need to define a lot of float variables, which takes up 90% of the time.
I have tested for 45 float variables, where only 16 get an initial value. The rest is just float z1; float z2;, etc. This took around 5-10 milliseconds, according to System.currentTimeMillis().
But this code with the 45 floats is in a method called by a double loop. In total this method is called 49 times (7*7). All this is inside the JOGL method to draw the game in a JFrame, but because of this many float variables it takes a total of 100ms, which means only 10fps.
So basically the problem is that I have to initialize 45*49=2205 floats. Is there any way to optimize this to get a better fps?
For example, would a double be faster than a float? Or would it help to define the variables first outside the loop, and give them their value inside the loop? Does anyone know a way to make this code run faster? Thanks a lot in advance.
EDIT
As requested, here is the source code:
for (int x = -4; x < 3; x++) { // Loops 7 times
for (int y = -4; y < 3; y++) { // Loops 7 times
long t1 = System.currentTimeMillis();
float z0 = terrain.getHeight(x-1, y-1); // Simple method, but takes up about half of the time
float z1 = terrain.getHeight(x , y-1);
float z3 = terrain.getHeight(x+1, y-1);
float z4 = terrain.getHeight(x+2, y-1);
float z5 = terrain.getHeight(x-1, y );
float z6 = terrain.getHeight(x , y );
float z7;
float z8;
float z9;
float z10 = terrain.getHeight(x+1, y );
float z11 = terrain.getHeight(x+2, y );
float z12;
float z13;
float z14;
float z15;
float z16;
float z17;
float z18;
float z19;
float z20;
float z21;
float z22;
float z23;
float z24;
float z25;
float z26;
float z27;
float z28;
float z29;
float z30;
float z31;
float z32;
float z33 = terrain.getHeight(x-1, y+1);
float z34 = terrain.getHeight(x , y+1);
float z35;
float z36;
float z37;
float z38 = terrain.getHeight(x+1, y+1);
float z39 = terrain.getHeight(x+2, y+1);
float z40 = terrain.getHeight(x-1, y+2);
float z41 = terrain.getHeight(x , y+2);
float z43 = terrain.getHeight(x+1, y+2);
float z44 = terrain.getHeight(x+2, y+2);
t1 = System.currentTimeMillis() - t1;
// Some other code where I use these variables.
// Takes between 0-1 ms in total.
}
}
EDIT
I now tested the getHeight() method, and it takes up about half of the time. The seven variables which use this method add up to about 5 ms, where the total is 10. The following is the code used in getHeight():
public float getHeight(float x, float y) {
long t1 = System.currentTimeMillis();
Coordinate c = new Coordinate(x, y);
for (Entry<Coordinate, Float> e : heightMap.entrySet()) { // heightMap = HashMap<Coordinate, Float>
if (e.getKey().x == c.x && e.getKey().y == c.y) {
System.out.println("getHeight: " + (System.currentTimeMillis() - t1) + " ms");
return e.getValue();
}
}
return 0f;
}
Coordinate is a class I made myself, it has a constructor with two float parameters for x and y, and saves them public, globally in the class itself.
The reason why I am not using heightMap.get(c), is because this always throws a NullPointerException, while the code given above never reaches the last line of return 0f;.
EDIT
Found the solution to the problem in this [link] (Why are custom objects not equivalent keys for a HashMap?) question, namely that I had to add public boolean equals(Object other) and public int hashCode() to my custom Coordinate class. Now the getHeight method can work with heightMap.get(c), which removes the loop in there and makes the program a lot faster. The total (with 49 loops) takes around 1 ms now.

Please note that Full Screen Exclusive Mode must be used for some operating systems to give you enough resources.
Defining variables outside of the loop will not help, as Java optimizes your code and defining variables inside a loop actually gives Java hints to increase performance. What I think (and I can only guess, since you posted no code) is, that you may consider using an array of longs. They are very effective to work with in a loop and they're also allocated one after another in your memory, so cache can be used effectively.
To me, fillings 2025 Longs takes slightly above one millisecond, including calls to random.nextLong() method.
public Long fillLongs(int numberofLongs) {
long[] longs = new long[numberofLongs];
Random r = new Random();
long start = System.currentTimeMillis();
for (long l : longs) {
l = r.nextLong();
}
long end = System.currentTimeMillis();
return end - start;
}
Using parallel stream, this task takes even less time. Often under 1 ms.
public Long fillLongs(int numberofLongs) {
Long[] longs = new Long[numberofLongs];
List<Long> longList = Arrays.asList(longs);
Random r = new Random();
long start = System.currentTimeMillis();
longList.parallelStream().forEach(l -> {
l = r.nextLong();
});
long end = System.currentTimeMillis();
return end - start;
}

For high-performance computing in Java you might consider usi JNI (Java Native Interface) - though it requires C++ knowledge. For a quick start take a look here.