Lesson-5.markdown

@@ -197,7 +197,28 @@ private void calcMotorValues(float interval)
 }
 ```
 
-The control loop calculates the average period for the controlloop, then gyro and motor values. It then calculates the output ..
+The control loop calculates the average period for the controlloop, then gyro and motor values. It then calculates and applies the control output:
+
+```java
+while(running)
+{
+	interval = calcInterval(i, startTime);
+	calcGyroValues(interval);
+	calcMotorValues(interval);
+	
+	float fpower = (KGYROSPEED * gyroSpeed +
+             	   KGYROANGLE * gyroAngle) / WHEELRATIO +
+             	   KPOS       * motorPosition  +
+             	   KSPEED     * motorSpeed;
+				
+	int power = Math.min(100, Math.abs((int)fpower));
+	int direction = fpower < 0 ? MotorPort.FORWARD : MotorPort.BACKWARD;
+	
+	leftMotor.controlMotor(power, direction);
+	rightMotor.controlMotor(power, direction);
+	i++;
+}
+```
 
 
 // STIKORD:::