From 225e3d4e5e5aa3fb1e2d111f0c2ed4046d67528b Mon Sep 17 00:00:00 2001
From: Tord <48350530+tordnat@users.noreply.github.com>
Date: Wed, 13 Mar 2024 03:00:21 +0100
Subject: [PATCH] Add: watchdog timer for moving elevator (#11)

Times out after x seconds of movement without stopping
---
 elevatorAlgorithm/elevator/elevator.go |  2 +-
 elevatorAlgorithm/fsm/fsm.go           | 12 +++++++++++-
 elevatorAlgorithm/hra/hra.go           |  1 -
 main.go                                |  1 -
 4 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/elevatorAlgorithm/elevator/elevator.go b/elevatorAlgorithm/elevator/elevator.go
index 957610e..df4cb5c 100644
--- a/elevatorAlgorithm/elevator/elevator.go
+++ b/elevatorAlgorithm/elevator/elevator.go
@@ -8,7 +8,7 @@ const N_FLOORS = 4
 const N_BUTTONS = 3
 const N_HALL_BUTTONS = 2
 const DOOR_OPEN_DURATION_S = 2
-
+const DOOR_OBSTRUCTION_TIMEOUT = 7
 type ElevatorBehaviour int
 
 const (
diff --git a/elevatorAlgorithm/fsm/fsm.go b/elevatorAlgorithm/fsm/fsm.go
index da5888f..5ad16f2 100644
--- a/elevatorAlgorithm/fsm/fsm.go
+++ b/elevatorAlgorithm/fsm/fsm.go
@@ -9,6 +9,8 @@ import (
 	"time"
 )
 
+var movingWatchdog *time.Timer
+
 func FSM(orderAssignment chan [][]bool, clearOrders chan requests.ClearFloorOrders, floorEvent chan int, obstructionEvent chan bool, elevStateToSync chan elevator.ElevatorState) {
 	elevState := elevator.ElevatorState{elevator.EB_Idle, -1, elevio.MD_Stop, [][]bool{
 		{false, false, false},
@@ -18,8 +20,10 @@ func FSM(orderAssignment chan [][]bool, clearOrders chan requests.ClearFloorOrde
 
 	doorTimer := time.NewTimer(0 * time.Second)
 	obstructedTimer := time.NewTimer(0 * time.Second)
+	movingWatchdog = time.NewTimer(0*time.Second)
 	<-doorTimer.C       // Drain channel
 	<-obstructedTimer.C // Drain channel
+	<-movingWatchdog.C
 	log.Println("Initializing Elevator FSM")
 	elevState = onInitBetweenFloors(elevState)
 	for elevio.GetFloor() == -1 {
@@ -39,6 +43,7 @@ func FSM(orderAssignment chan [][]bool, clearOrders chan requests.ClearFloorOrde
 			clearOrders <- OrdersToClear(elevState) // This must be run last to not clear orders at the wrong place
 
 		case floor := <-floorEvent:
+			elevio.SetFloorIndicator(floor)
 			elevState.Floor = floor
 			var clearOrder requests.ClearFloorOrders
 			elevState.Behaviour, clearOrder = OnFloorArrival(elevState, doorTimer)
@@ -60,7 +65,7 @@ func FSM(orderAssignment chan [][]bool, clearOrders chan requests.ClearFloorOrde
 			if elevState.Behaviour == elevator.EB_DoorOpen {
 				if isObstructed {
 					log.Println("Obstruction occured!")
-					obstructedTimer.Reset(time.Second * 7)
+					obstructedTimer.Reset(time.Second * elevator.DOOR_OBSTRUCTION_TIMEOUT)
 				} else {
 					obstructedTimer.Stop()
 				}
@@ -68,6 +73,8 @@ func FSM(orderAssignment chan [][]bool, clearOrders chan requests.ClearFloorOrde
 			}
 		case <-obstructedTimer.C:
 			os.Exit(1)
+		case <-movingWatchdog.C:
+			os.Exit(2)
 		}
 		elevStateToSync <- elevState //Must find out if this is a good place to sync elevState
 	}
@@ -94,6 +101,7 @@ func updateOrders(elevState elevator.ElevatorState, doorTimer *time.Timer) (elev
 				elevio.SetDoorOpenLamp(true)
 				doorTimer.Reset(elevator.DOOR_OPEN_DURATION_S * time.Second)
 			case elevator.EB_Moving:
+				movingWatchdog.Reset(time.Second * 10)
 				elevio.SetMotorDirection(elevState.Direction)
 			}
 		}
@@ -117,6 +125,7 @@ func OnFloorArrival(elevState elevator.ElevatorState, doorTimer *time.Timer) (el
 	switch elevState.Behaviour {
 	case elevator.EB_Moving:
 		if requests.ShouldStop(elevState.Direction, elevState.Floor, elevState.Requests) {
+			movingWatchdog.Stop()
 			elevio.SetMotorDirection(elevio.MD_Stop)
 			clearOrder.Floor = elevState.Floor
 			clearOrder.Cab = true
@@ -145,6 +154,7 @@ func OnDoorTimeOut(elevState elevator.ElevatorState, doorTimer *time.Timer) (ele
 			elevio.SetDoorOpenLamp(false)
 			elevio.SetMotorDirection(elevState.Direction)
 		case elevator.EB_Moving:
+			movingWatchdog.Reset(time.Second * 10)
 			elevio.SetDoorOpenLamp(false)
 			elevio.SetMotorDirection(elevState.Direction)
 		}
diff --git a/elevatorAlgorithm/hra/hra.go b/elevatorAlgorithm/hra/hra.go
index 0e209ff..7fced00 100644
--- a/elevatorAlgorithm/hra/hra.go
+++ b/elevatorAlgorithm/hra/hra.go
@@ -94,7 +94,6 @@ func Encode(system ElevatorSystem) string {
 func AssignRequests(elevatorStates string) string {
 	out, err := exec.Command("./hall_request_assigner", "--includeCab", "-i", (elevatorStates)).Output()
 
-
 	if err != nil {
 		fmt.Println("Error ", err)
 	}
diff --git a/main.go b/main.go
index bba9b2e..ec00d61 100644
--- a/main.go
+++ b/main.go
@@ -19,7 +19,6 @@ func main() {
 	log.Println("Elevator starting 🛗")
 	elevatorPort, elevatorId := cmd.InitCommandLineArgs(os.Args)
 	elevio.Init(fmt.Sprintf("localhost:%d", elevatorPort), elevator.N_FLOORS)
-
 	buttonEvent := make(chan elevio.ButtonEvent)
 	floorEvent := make(chan int)
 	obstructionEvent := make(chan bool)