Lesson-8-Journal-2.markdown

@@ -14,7 +14,7 @@ The goal for the day is to make a Lego car follow the Alishan track using a gyro
 
 ## Plan
 
-The plan for the day is to build upon previous results [1]. It seemed plausible to use a gyroscope to detect when a plateau is reached however additional sensory needs to be implemented in order for the car to follow the straight inclined paths. The idea is to exploit the black line on these paths and use a light sensor to make the car follow these lines. Behavioral control will be necessary in this setup due to the car having multiple behaviors. It is estimated that hard coding is the optimal solution for the turns do the course of the black line.  
+The plan for the day is to build upon previous results [1]. It seemed plausible to use a gyroscope to detect when a plateau is reached however additional sensory needs to be implemented in order for the car to follow the straight inclined paths. The idea is to exploit the black line on these paths and use a light sensor to make the car follow these lines. Behavioral control will be necessary in this setup due to the car having multiple behaviors. It is estimated that hard coding is the optimal solution for the turns do the course of the black line.
 
 
 ## Implementing a light sensor
@@ -106,11 +106,53 @@ switch (nextAction) {
 }		
 ```
 The FSM is responsible for performing all turns on the track. By letting the FSM control the stopping mechanism we avoid having a third behavior that uses the light sensor or an additional color sensor to detect when the LEGO car is in the green end zone. 
-
+	
 Each turn is hard coded by imposing a certain power to each motor in a fixed amount of time. The amount of power and the time interval is then adjusted according to manual inspection. 
 
 The hardware setup is identical to previously. 
 
+The PlateauPilot performs an action every time a plateau is reached. This is sensed using the gyroscope. In the code snippet below the run function of the PlateauPilot is shown. This function sets the boolean ´isPlatueaReached´, which triggers the Arbiter to perform the next action, as described by the state machine above.
+
+```java
+public void run() {
+	while(true)
+	{
+		int gyroVal = gyro.readValue();
+		double filteredVal = oldFilteredVal*alpha + gyroVal*(1-alpha);
+		oldFilteredVal = filteredVal;
+		
+		switch (state) {
+		case idle: //starting in idle
+			if (filteredVal > gyroOffset + 40 && isAscending)
+			{
+				if(System.currentTimeMillis() > timePlateauReached + 2000) {
+					state = State.plateauReached;
+					isPlateauReached = true;
+				}
+			}
+			else if (gyroVal < gyroOffset - 40 && !isAscending)
+			{
+				if(System.currentTimeMillis() > timePlateauReached + 2000) {
+					state = State.plateauReached;
+					isPlateauReached = true;
+				}
+			}
+			break;
+		case plateauReached:
+			if (!isPlateauReached) {
+				state = State.idle;
+				timePlateauReached = System.currentTimeMillis();
+			}
+			break;
+		default:
+			break;
+		}
+	}
+}
+```
+The gyro values are filtered with an exponential filter to remove unwanted spikes. If the filtered value is 40 degress/second above or below the offset, a plateau is either entered or left. In order not to trigger the next action when leaving a plateau a timing threshold is inserted. This ensures that at least two seconds passes between actions.
+
+
 ### Results
 
 With the current setup the car is able to follow the black line on the first inclined path. However a problem arises in the plateau detection which is illustrated in the following figure.