Lesson-5.markdown

@@ -152,17 +152,70 @@ Self-balancing robots with gyro sensor
 
 ### 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].
+#### 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: 
+
+```java
+void calcGyroValues(float interval)
+{
+   	int gyroRaw = gyro.readValue();
+    	
+   	// adjust the offset dynamically to correct for the drift of the sensor
+   	offset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * offset;
+   	
+   	//calculate speed and angle. Interval is the actual time for the last loop iteration.
+   	gyroSpeed = gyroRaw - offset;    	
+   	gyroAngle += gyroSpeed*interval;    
+}
+```
+
+The motor terms `motorSpeed` and `motorPosition` is calculated as shown in the following listing:
+
+```java
+private void calcMotorValues(float interval) 
+{
+   	//get encoder values
+   	int left = leftMotor.getTachoCount();
+   	int right = rightMotor.getTachoCount();
+   	
+   	//position is the sum of the two encoder values
+   	int sum = left + right;
+   	int delta = sum - oldMotorSum;
+   	motorPosition += delta;
+   	
+   	//calculate motorspeed as the average motor speed of the last 4 iterations.
+   	motorSpeed = (delta + old1MotorDelta + old2MotorDelta + old3MotorDelta) 
+   				/ (4 * interval);
+    	
+   	// Save old values for future use
+   	oldMotorSum = sum;
+   	
+   	old3MotorDelta = old2MotorDelta;
+   	old2MotorDelta = old1MotorDelta;
+   	old1MotorDelta = delta;  	
+}
+```
+
+The control loop calculates the average period for the controlloop, then gyro and motor values. It then calculates the output ..
 
 
 // STIKORD:::
 
 /// Indsæt billede
 
-Sensoren har siddet to steder, først i toppen og dernæst længere nede -> Mindre rystelser, men også mindre udsving i vinklen.
 
 
+### Results
+
+In order to investigate the properties of the gyro sensor, it was mounted on the shoulder of the robot. The robot was then rotated about 90 degrees in one direction (1. movement), then 180 degrees in the opposite direction (2. movement), then then rotated back 90 degrees to the starting point (3. movement). This procedure was carried out for all three axes, one at a time, as shown in video [Exercise 3.1].
+
+![Raw output from gyro sensor during testing.](https://gitlab.au.dk/rene2014/lego/raw/master/Lesson5/Images/GyroData.png)
+
+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.
+
+... drift .. 
+
 Forklar hvordan vi har isoleret de forskellige gains og hvad de gjorde.
 
 KGyroAngle = Positiv vinkel selvom den ikke burde/forkert vinkel -> Skru op for denne så vælter lortet. Robottens nulpunkt drifter
@@ -175,19 +228,7 @@ Hvad vi har prøvet:
 
 - Gyroangle                             -> Robotten reagere for langsomt
 - Afhjælpes med GyroSPeed Gain          -> Robotten reagere hurtigt, men den ved ikke hvad vinklen er
--  
-
-////
-
-### Results
-
-In order to investigate the properties of the gyro sensor, it was mounted on the shoulder of the robot. The robot was then rotated about 90 degrees in one direction (1. movement), then 180 degrees in the opposite direction (2. movement), then then rotated back 90 degrees to the starting point (3. movement). This procedure was carried out for all three axes, one at a time, as shown in video [Exercise 3.1].
-
-![Raw output from gyro sensor during testing.](https://gitlab.au.dk/rene2014/lego/raw/master/Lesson5/Images/GyroData.png)
-
-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.
-
-... drift .. 
+- Sensoren har siddet to steder, først i toppen og dernæst længere nede -> Mindre rystelser, men også mindre udsving i vinklen.
 
 ## Conclusion
 
@@ -204,7 +245,6 @@ Bedre gyro -> Sensor fusion
 
 [4] https://gitlab.au.dk/rene2014/lego/tree/master/Lesson5/Programs/SegwayOnTheFlyNXT
 
-
 [5] http://www.hitechnic.com/blog/gyro-sensor/htway/
 
 [6] https://gitlab.au.dk/rene2014/lego/tree/master/Lesson5/Programs/SegwayGyroSensor