Session 9
Date: 16/06 2014
Duration of activity: 9.15 - 15.30
Group members participating: Pætur Askildsen, Christian Jegstrup Hansen, Søren Gregersen, Søren Ditlev and Alexander Rasmussen
Goal
Plan
Results
Work on gyro drift so far
To sum up, we have found that the gyro sensor readings for the HiTechnic NGY1044 gyro sensor are affected by changes in input voltage, which happens when the motors are turned on [1][2][3]. Kirk Thomson seems to be the first one to notice this in 2007 [3], when he was building a Gyro class, probably the one we use in our project as he is co-author of that one [4]. Everything was working fine for Thompson until he coded to also run a motor with a PID-controller together with the gyroscope. He found that his gyro angle integrator would drift after setSpeed() was called: “The sensor value (from the ADC) changes by 2 after setSpeed() is called”. He therefore advises to: “The solution was to recalc the bias [gyro sensor offset, red.] after setSpeed(0) was called” [3].
We, however, do not use the same setSpeed() method as Thompson does because LeJOS motor classes has changed since 2007. Which motor should we use? We do not want a regulated motor because under LeJOS motor tutorial under the headline ‘When might you want to turn off speed regulation?’ [4] it states that if speed corrections happen frequently, like in a line follower or a balancing robot, there is no advantage in the regulator thread which tries to keep the speed constant. Instead an unregulated motor could be used, creating an instance of the NXTMotor class, for example:
NXTMotor m1 = new NXTMotor(MotorPort.A);
This is exactly what we do, as we work with a balancing robot, where corrections happen frequently and the speed should not be constant.
We rewrote Ole Caprani’s Car class [6] to a TubeCar class using the NXTMotor class, see figure 1. NXTMotor class does not have a setSpeed() method but a setPower() method. We made a test to see if setPower(0) changes the gyro sensor value readings in the same way, by 2, as setSpeed(0) by finding code inspiration in Thompson initial test [3]. We made a test program, and found that setPower(0) like setSpeed(0) causes the gyro sensor reading to drop by 2, using a DataLogger class, see figure 2 for a graph of the test results of the DataLogger illustrating the gyro sensor readings drop by 2. See figure 3 for Thompson’s modified test class. The random fluctuations away from 591, before the method call, and 589 after the method call is caused by the fact that the offset of a gyro will vary a bit over time, which is called random walk or drift [1].
Figure 1: TubeCar.java which we use in our tube-balancing car.
Figure 2: A test made which shows, that setPower(0) actually causes a drop in gyro sensor readings by approximately 2 (from 591 to 589), making it the right method to call before gyro sensor offset calibration.
Figure 3:
The program used to test if setPower(0) affects raw gyro sensor readings. Start program, wait a few seconds, hit and hold the enter button for a short moment to ensure that the if-loop is called (as the thread is set to sleep sometimes), wait the same amount of seconds, and hit escape to stop logging the data. To avoid the need to hold the enter button to ensure that it is registered that it has been pressed, we could have used a ButtonListener [7], which: “ [...] is used to detect when a button is pressed, whatever your program is doing at the time” but it was not deemed necessary time-wise to implement for this test as long as we are aware of the fact, that you have to hold the enter button for a little while.
If the gyro sensor readings drop by 2 when setPower(0) is called, is the same constant drop of 2 noticed when setPower(10) or setPower(50) instead is called? We tried this out, changing the setPower(0) to setPower(50). The results can be seen in figure 4 below. Before calling setPower(50) the gyro sensor value is around 591 with some random walk. But after setPower(50) is called it is very difficult to get a good reading, because setPower() sets and applies the power to the motors, where the vibrations from the motors running causes the oscillations seen after setPower(50) is called even though we tried to lay the car where the wheels could spin in free air.
Figure 4: A test which is unable to show if setPower(50) like setPower(0) causes a drop in gyro sensor readings by approximately 2 (from 591 to 589).
Therefore we tried a third test where we set the power to 20 so it is different from 0 but the motors wont run. It seems that instead of a drop of 2 from 591 to 589 it drops more often to 590, by 1. But it seems not to be significant, since it also drops to 589 sometimes. We stop making tests here of time considerations. Future work would be to log gyro sensor reading drops for different setPower() values and make regression on this to maybe have a formula to try out use for adjusting the offset, if gyro sensor drop is found to not be constant of 2.
Figure 5: A test to show if setPower(20) causes a drop in gyro sensor readings by 2.
Another trick to avoid the changing gyro sensor offset as a result of power drops, mentioned in [1], is to use a sensor mux with an external power supply, for example like the one from Mindsensors, see figure 6. This way the motor and the gyro gets power from two different sources and thus does not impact each other. We however chose for the sake of simplicity to go with powering the motors and the gyroscope from the same NXT battery, keeping in mind powering on the motors before calibrating and using the gyroscope. In session 1 we mentioned that [1] advices to choose another gyroscope than the HiTechnic, but since other have build balancing robots with the HiTechnic gyroscopes, we still chose to go with those.
Figure 6: This multiplexer can connect any sensor designed for Mindstorms NXT (LEGO and/or third party), upto 4 sensors to one NXT sensor Port, uses External Battery [8].
One of the biggest factors contributing to gyro sensor drift is temperature. When one starts using a sensor, the temperature of the sensor rises, and it gets hotter than the ambient temperature. This results in the offset (the sensor value readings) of the sensor changing [1][9]. Therefore [1] advices to let the sensor warm up before calculating the offset, if you need a very good signal. Even when temperature and power are constant the offset of a gyro will vary a bit over time, as also could be seen in the graph in figure 2, the random walk variation.
Work on reducing gyro drift
As the gyro sensor readings change when they gyro sensor gets hot, we worked on implementing a method they use in the original HTWay NXC code for continuously correcting this drift. We previously turned this off since we thought that it caused more drift. But in this lesson we tried to turn it on again, to give it another try and tweak the value, since the gyro sensor changes readings over time when it gets hot, linearly changing the offset is a need, as mentioned in lesson 4. In the original HiTechnic HTWay code that we used in session 2, they in their comments describe their way for coping with gyro drift [10]:
and they use this constant in:
Where gOffSet is our initial bias / offset value from 100 gyro samples:
The first test we did after implementing the drift regulation in our robot, the robot quickly drifted the opposite way of what we had seen in earlier tests. This let us to believe that the EMAOFFSET value was too high. We then slowly decreased the value, while testing it each time, and ended up with a value of 0.0007.
This implementation still made the sensor drift, but the drift was now slower, and the robot, stayed on the tube for around 30 seconds longer, than what we had achieved before.
Accelerometer slowing motors down
When testing the filter where we also use the accelerometer, we discovered a bug that we have yet to figure out. When the accelerometer is on the reaction time of the balancing robot is very much decreased causing the robot to react slowly thus falling off the tube much quicker than before. When we do not call for accelerometer readings and do calculation on those readings, the motors reacts as quick as before. We think this might be due to the added sensor fetching and sensor value calculations in our control loop causing the loop to take longer to go through resulting in less often motor power and direction commands, resulting in a slower reacting balancing robot.
Continued work on the complementary filter
In session 6 we introduced the complementary filter and the theory behind it. Our initial code from session 6 however did not work as intended, as the angle calculated in the filter did not react fast enough on the gyroscope values.
After doing a little more research on the complementary filter, we found out that our approach to how the accelerometer value is calculated was wrong. Instead of calculating a gravity vector based on readings from two axes as we did in session 6, we found another approach which is described below.
The accData variable in the complementary filter equation is the data from the accelerometer processed according to the following formula:
accData=Math.toDegrees((Math.atan2 (accX, accZ) + Math.PI))-accOffset
In the formula we only worry about getting the result from the y-axis because it is along this axis the robot is moving.
As earlier described the acceleration is measured in three axes, x, y and z. Acceleration is measured in the range of –2g to +2g with scaling of approximately 200 counts per g [4]. To obtain the angular position with the accelerometer, we are going to determine the position of the gravity vector (g-force) which is always visible on the accelerometer.
We can do this by using the method atan2 provided in the Math class. The atan2 method converts the rectangular coordinates (x, y) to polar coordinates (r, theta) and returns the angle theta. By calling the Math.toDegrees() we change the polar coordinates returned by the atan2 function to degrees, so that both the accelerometer data and gyroscope data is calculated in degrees.
To calibrate the accelerometer to be 0 at the starting point, we subtract a constant, accOffset, from the calculated number. The offset has been found by placing the car on a horizontal surface while reading the accData constant.
Figure 7: The implementation of the complementary filter.
This implementation still turned out to cause a few problems. Or at least it did not work as intended. We are quite sure that the values calculated in the algorithm are correct, but there is still a problem with the angle that the filter returns to the PID controller of the main program. When the filter is tested on the robot without activating the motors it works great. But when the motors are activated it seems that they interrupt the filter.
Conclusion
We have tried to minimize the gyro drift by continuously regulating the offset, but did not get the a satisfying result, since the drift seem to vary a lot in each individual test.
We have experimented with how activating the motors influence at high power will influence the gyroscope, as previously we concluded that setting motor power to 0 produces a drop of 2 in pure gyro readings. We found that by setting the motor power to 20 the system still produces a small drop in gyro reading (1-2), but that system produces notably more noise at this power level. With the motors at 50 the system was to noise for us to get reliable reading, therefor we will continue use the 2 reading drop in our project, though further test can be done at this area.
We have change the way in which we calculate the reading from the accelerometer, to fit into our complementary filter, theoretically this should produce better result, but it need further testing before we can confirm this.
Plan for next session(s)
Test and fine tune the complimentary filter
References
[5] Gyrosensor output changes with motorpower, https://sites.google.com/site/legolabnotebook/end-course-notebook/session-3/session-4
[9] http://www.lejos.org/forum/viewtopic.php?f=7&t=4656#p19823
[10] HiTechnic HTWay Version 1.1, http://www.hitechnic.com/upload/786-HTWayC.nxc
