Lesson-5.markdown

@@ -152,32 +152,59 @@ 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();
     	
-// STIKORD:::
+   	// adjust the offset dynamically to correct for the drift of the sensor
+   	offset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * offset;
    	
-/// Indsæt billede
+   	//calculate speed and angle. Interval is the actual time for the last loop iteration.
+   	gyroSpeed = gyroRaw - offset;    	
+   	gyroAngle += gyroSpeed*interval;    
+}
+```
 
-Sensoren har siddet to steder, først i toppen og dernæst længere nede -> Mindre rystelser, men også mindre udsving i vinklen.
+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;  	
+}
+```
 
-Forklar hvordan vi har isoleret de forskellige gains og hvad de gjorde.
+The control loop calculates the average period for the controlloop, then gyro and motor values. It then calculates the output ..
 
-KGyroAngle = Positiv vinkel selvom den ikke burde/forkert vinkel -> Skru op for denne så vælter lortet. Robottens nulpunkt drifter
-KGyroSpeed = Fungerer fint. Jo højere værdi jo hurtigere reagere robotten
-KSpeed = Jo hurtigere hjulet drejes jo mere modstand gives i motoren.
-KPos = VIrker som forventet. Bestemmer en gain faktor i forhold til hvor hurtigt motorene finder tilbage til set point
 
+// STIKORD:::
 
-Hvad vi har prøvet:
+/// Indsæt billede
 
-- Gyroangle                             -> Robotten reagere for langsomt
-- Afhjælpes med GyroSPeed Gain          -> Robotten reagere hurtigt, men den ved ikke hvad vinklen er
--  
 
-////
 
 ### Results
 
@@ -189,6 +216,20 @@ The data shows, that the offset for this sensor is around 600. Furthermore, it c
 
 ... 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
+KGyroSpeed = Fungerer fint. Jo højere værdi jo hurtigere reagere robotten
+KSpeed = Jo hurtigere hjulet drejes jo mere modstand gives i motoren.
+KPos = VIrker som forventet. Bestemmer en gain faktor i forhold til hvor hurtigt motorene finder tilbage til set point
+
+
+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
+- Sensoren har siddet to steder, først i toppen og dernæst længere nede -> Mindre rystelser, men også mindre udsving i vinklen.
+
 ## Conclusion
 
 Bedre gyro -> Sensor fusion 
@@ -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