From 472eba7977bfefebc1c999650a1c143ffca106f9 Mon Sep 17 00:00:00 2001
From: Simon Schmidt <schmidtsi76327@th-nuernberg.de>
Date: Sat, 3 Jul 2021 12:48:17 +0200
Subject: [PATCH] updates to state machine with print function

---
 lib/treppe/treppe.cpp | 48 ++++++++++++++++++++++++++++++++++---------
 lib/treppe/treppe.h   | 24 +++++++++++++++-------
 2 files changed, 55 insertions(+), 17 deletions(-)

diff --git a/lib/treppe/treppe.cpp b/lib/treppe/treppe.cpp
index f9c6738..7d31847 100644
--- a/lib/treppe/treppe.cpp
+++ b/lib/treppe/treppe.cpp
@@ -132,10 +132,8 @@ void Treppe::rampe()
 }
 
 void Treppe::setup(){
-    Serial.printf("differenz_pwm_pro_tick %f\n", differenz_pwm_pro_tick);
-   
+
     pwmController.resetDevices();
-    
     // Deactive PCA9685 Phase Balancer due to LED Flickering
     // https://github.com/NachtRaveVL/PCA9685-Arduino/issues/15
     // see also lib/PCA9685-Arduin/PCA9685.h:204
@@ -145,27 +143,57 @@ void Treppe::setup(){
     pwmController.setAllChannelsPWM(idle_brightness);
     
     pinMode(A0, INPUT);
-    pinMode(SENSOR1, INPUT);
-    pinMode(SENSOR2, INPUT);
+    pinMode(SENSOR_OBEN, INPUT);
+    pinMode(SENSOR_UNTEN, INPUT);
     pinMode(OE, OUTPUT);
     digitalWrite(OE, 0);
+
+    Serial.printf("differenz_pwm_pro_tick %f\n", differenz_pwm_pro_tick);
     Serial.println("Hello from Treppe");
     Serial.print("Treppe: initial parameters: stairs=");
     Serial.println(stairs);
 }
 
-void Treppe::task(){
-  FSMTreppe_Obj.step();
+void Treppe::print_state_on_change() {
+  static FSMTreppeModelClass::ExtU_FSMTreppe_T last_in;
+  static FSMTreppeModelClass::ExtY_FSMTreppe_T last_out;
+  if(
+    fsm_inputs.anim_finished != last_in.anim_finished || 
+    fsm_inputs.sensor_oben != last_in.sensor_oben || 
+    fsm_inputs.sensor_unten != last_in.sensor_unten || 
+    fsm_outputs.anim_active != last_out.anim_active
+  ) {
+    last_in.anim_finished = fsm_inputs.anim_finished;
+    last_in.sensor_oben  = fsm_inputs.sensor_oben;
+    last_in.sensor_unten = fsm_inputs.sensor_unten;
+    last_out.anim_active = fsm_outputs.anim_active;
 
+    Serial.printf("FSM inputs: s_u: %d, s_o: %d, an_fin: %d =>", 
+      fsm_inputs.sensor_oben, fsm_inputs.sensor_unten, fsm_inputs.anim_finished);
+    Serial.print(" step => ");
+    Serial.printf("FSM outputs: an_act: %d\n", fsm_outputs.anim_active);
+  }
+}
+
+void Treppe::task(){
   if(finish){
     direction = switch_direction;
     state = switch_state;
   }  
   static uint8_t last_sensor_state[2] = {0,0}; 
   uint8_t current_sensor_state[2] = {0,0};
-  current_sensor_state[0] = digitalRead(SENSOR1);
-  current_sensor_state[1] = digitalRead(SENSOR2);
-  
+  current_sensor_state[0] = digitalRead(SENSOR_OBEN);
+  current_sensor_state[1] = digitalRead(SENSOR_UNTEN);
+
+  fsm_inputs.sensor_oben =  read_sensor(SENSOR_OBEN);
+  fsm_inputs.sensor_unten = read_sensor(SENSOR_UNTEN);
+  fsm_inputs.anim_finished = static_cast<bool>(finish);
+
+  FSMTreppe_Obj.setExternalInputs(&fsm_inputs);
+  FSMTreppe_Obj.step();
+  fsm_outputs = FSMTreppe_Obj.getExternalOutputs();
+  print_state_on_change();
+
   if(current_sensor_state[0] && !last_sensor_state[0] && state == 0){
     setTick(0);
     setAnAus(1);
diff --git a/lib/treppe/treppe.h b/lib/treppe/treppe.h
index a7efcbb..b5fefd8 100644
--- a/lib/treppe/treppe.h
+++ b/lib/treppe/treppe.h
@@ -3,22 +3,19 @@
 #include "FSMTreppe/FSMTreppe.h"
 #include "PCA9685.h"
 
-#define SENSOR1 2
-#define SENSOR2 12
+#define SENSOR_OBEN 2
+#define SENSOR_UNTEN 12
 #define OE 14
 
 #define INT_TIME 20  // interrupt intervall [ms]
 
 class Treppe {
 private:
-    // initialize with i2c-Address 0, use Wire Library
-    PCA9685 pwmController;     
-    FSMTreppeModelClass FSMTreppe_Obj;// Instance of model class
 
     uint8_t stairs;
     uint16_t time_per_stair = 300;          // dimmtime per stair [ms]
     uint16_t idle_brightness = 100;
-    uint16_t active_brightness = 2048;
+    uint16_t active_brightness = 300;
 
     uint8_t direction = 0;
     uint8_t switch_direction = 0;
@@ -36,14 +33,27 @@ private:
     float differenz_pwm_pro_tick = 0.0;
     // alternative
 
+    // initialize with i2c-Address 0, use Wire Library
+    PCA9685 pwmController;     
+    FSMTreppeModelClass FSMTreppe_Obj;
+    FSMTreppeModelClass::ExtU_FSMTreppe_T fsm_inputs;
+    FSMTreppeModelClass::ExtY_FSMTreppe_T fsm_outputs;
+    void print_state_on_change();
+
+
     uint8_t softstart_led(uint8_t led, uint16_t startval, uint16_t stopval);
     void ledsequence();
     void rampe();
-
+    bool read_sensor(int sensor) {
+        int pegel = !digitalRead(sensor);
+        return static_cast<bool>(pegel);
+    }
 public:
     Treppe(uint8_t _stairs) : stairs(_stairs){
         FSMTreppe_Obj.initialize();
 
+
+
         ticks_pro_stufe = time_per_stair / 20; // [ms]
         ticks_treppe = ticks_pro_stufe * stairs;