| ... | ... | @@ -152,9 +152,16 @@ Self-balancing robots with gyro sensor |
|
|
|
|
|
|
|
### Setup
|
|
|
|
|
|
|
|
The controller used for this exercise is inspired by the controller described in [5]. Unlike previous exercises, this controller is not a regular PID controller, and it monitors two values instead of one; angular velocity and tacho counts of the two motors. The angular velocity is then integrated over time, to obtain the current angle of the robot. The current motor position is calculated as the sum of the tacho count from each motor. The current motor speed is then derived from the position by differentiating the position with regards to time. The controller is thus relying on four terms:
|
|
|
|
|
|
|
|
* **GyroAngle:** This term is responsible for causing the robot to return to the starting position (standing upright). If the robot starts in the upright position, that angle will be 0 degrees. This value will be positive if the robot is leaning forward, and negative if its leaning backward, thus contributing to the control output.
|
|
|
|
* **GyroSpeed:** This term is non-zero when the robot is accelerating, and thus contributes to the control output when the robot is falling.
|
|
|
|
* **MotorPosition:** This term is non-zero when the wheels of the robot has turned from the starting point. Thus, this term is responsible for keeping the robot stationary, and is not related to staying upright. This term can be used to drive the robot to a desired location, however we did not exploit this fact in this exercise.
|
|
|
|
* **MotorSpeed:** This term is non-zero when the wheels of the robot are rotating. This term keeps the robot from oscillating.
|
|
|
|
|
|
|
|
#### Software Setup
|
|
|
|
|
|
|
|
The code for this exercise is inspired by the code described and linked in [5]. The full implementation can be found at [6], with the corresponding PC program located at [7]. The important parts of the logic are shown in the this section. The following listing how calculating angular velocity (`gyroSpeed`) and `gyroAngle` from the raw gyro output is performed:
|
|
|
|
The implementation of the controller used in this exercise is inspired by the one described and implemented in [5]. The full implementation can be found at [6], with the corresponding PC program located at [7]. The important parts of the logic are shown in the this section. The following listing shows how calculating angular velocity (`gyroSpeed`) and `gyroAngle` from the raw gyro output is performed:
|
|
|
|
|
|
|
|
```java
|
|
|
|
void calcGyroValues(float interval)
|
| ... | ... | @@ -179,7 +186,7 @@ private void calcMotorValues(float interval) |
|
|
|
int left = leftMotor.getTachoCount();
|
|
|
|
int right = rightMotor.getTachoCount();
|
|
|
|
|
|
|
|
//position is the sum of the two encoder values
|
|
|
|
//position is the sum of the two tacho counts
|
|
|
|
int sum = left + right;
|
|
|
|
int delta = sum - oldMotorSum;
|
|
|
|
motorPosition += delta;
|
| ... | ... | @@ -234,7 +241,7 @@ In order to investigate the properties of the gyro sensor, it was mounted on the |
|
|
|
|
|
|
|
|
|
|
|
The data shows, that the offset for this sensor is around 600. Furthermore, it can be seen from the data, that the supplied gyro sensor only senses changes to angular velocity in one axis. This can be seen by the three spikes starting just before T = 8. These spikes corresponds the three movements, for a single axis.
|
|
|
|
The small fluctuations in th graph is mainly due to an unsteady hand, when turning the robot. In order to test if the gyro drifts, a test was performed, where the gyro sensor was lying completely still. The results from this test is shown in the figure below.
|
|
|
|
The small fluctuations in the graph is mainly due to an unsteady hand, when turning the robot. In order to test if the gyro drifts, a test was performed, where the gyro sensor was lying completely still. The results from this test is shown in the figure below.
|
|
|
|
|
|
|
|
|
|
|
|
|
| ... | ... | |
