From b4067c8379ffc66d15ac5139784d83cac559b02e Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Thu, 23 Nov 2023 23:39:44 +0100
Subject: [PATCH] use duetimer and fix bug where if the next state is the same
 the exit function would still get called

---
 due_obd2/due_obd2.ino   | 59 ++++++++++++-----------------------------
 due_obd2/obd2_timer.c   |  4 +--
 due_obd2/statemachine.c |  9 +++++--
 3 files changed, 25 insertions(+), 47 deletions(-)

diff --git a/due_obd2/due_obd2.ino b/due_obd2/due_obd2.ino
index 918d5b0..a21d0bb 100644
--- a/due_obd2/due_obd2.ino
+++ b/due_obd2/due_obd2.ino
@@ -9,8 +9,8 @@ Program to create a car monitor display using:
 #include "Arduino.h"
 /* include UTFT library */
 #include <UTFT.h>
-// #include <DueTimer.h>
-#include "obd2_timer.h"
+#include <DueTimer.h>
+// #include "obd2_timer.h"
 #include "obd2_display.h"
 #include "statemachine.h"
 
@@ -22,6 +22,9 @@ Program to create a car monitor display using:
 #define PIN_BT_TX 18
 #define PIN_BT_STATE 3 /* connected/disconnected state of BT */
 
+#define MS(x) x * 1000
+#define S(x) MS(x) * 1000
+
 extern uint8_t BigFont[];
 extern uint8_t SmallFont[];
 
@@ -45,7 +48,7 @@ void __state_none(){ /* do nothing */}
 state_t init_state =
     {
         .id = STATE_INIT,
-        .next = STATE_INIT,
+        .next = STATE_CAR_INFO,
         .on_enter = &on_init_enter,
         .on_run = &on_init_run,
         .on_exit = &on_init_exit};
@@ -66,19 +69,12 @@ void on_init_enter()
 {
   display.clrScr();
   Serial.println("Entering init state!");
-  // obd2_TC_init();
-  // Serial.println("init done");
-  // obd2_TC_set_RC_value(1000000); /* 1 million microseconds? */
-  // Serial.println("set RC");
-  // obd2_TC_start();
-  // Serial.print("Timer initialized and started");
-  // Timer3.attachInterrupt(timer_handler);
-  // Timer3.start(50000);
 }
 
 void on_init_run()
 {
-  Serial.println("Running!");
+  delay(500);
+  Serial.println("init!");
 
 }
 
@@ -95,25 +91,13 @@ void on_main_enter()
 
 void on_main_run()
 {
-
-}
-
-// void RTT_Handler()
-// {
-//   flag = 0;
-//   obd2_RTT_reset();
-// }
-
-void TC0_Handler(void)
-{
-  #if (DEBUG == 1)
-    Serial.println("Interrupt!");
-  #endif
+  delay(500);
+  Serial.println("main!");
 }
 
 void timer_handler()
 {
-  Serial.println("Timer yeet");
+  statemachine_next();
 }
 
 void setup()
@@ -138,28 +122,19 @@ void setup()
 
   pinMode(LED_BUILTIN, OUTPUT);
 
-  // statemachine_register_state(&init_state, STATE_INIT);
-  // statemachine_register_state(&main_state, STATE_CAR_INFO);
-  // statemachine_init();
-
-
-  obd2_TC_init();
-  Serial.println("init done");
-  obd2_TC_set_RC_value(50000);
-  Serial.println("set RC");
-  obd2_TC_start();
-  Serial.print("Timer initialized and started");
-  // Timer3.attachInterrupt(timer_handler);
-  // Timer3.start(50000);
-  Serial.println("gagfadfgadfsd");
+  statemachine_register_state(&init_state, STATE_INIT);
+  statemachine_register_state(&main_state, STATE_CAR_INFO);
+  statemachine_init();
 
+  Timer3.attachInterrupt(timer_handler);
+  Timer3.start(S(5));
 
 }
 
 void loop()
 {
 
-  // statemachine_loop();
+  statemachine_loop();
   // Serial.println("Running");
   // delay(10);
   //   while (Serial1.available())
diff --git a/due_obd2/obd2_timer.c b/due_obd2/obd2_timer.c
index e722227..077bb2e 100644
--- a/due_obd2/obd2_timer.c
+++ b/due_obd2/obd2_timer.c
@@ -60,9 +60,7 @@ void obd2_TC_init()
   TC0->TC_CHANNEL[0].TC_IDR = 0xFFFFFFFF;
   /* Clear status register by reading it*/
   TC0->TC_CHANNEL[0].TC_SR;
-
-  /* disable the quadrature decoder, to route the IO pins of TIOA and TIOB directly to the timer counter function (36.6.14.1) */
-//   TC0->TC_BMR &= ~(TC_BMR_QDEN);
+  
   /* Set wave mode to UP mode with automatic trigger, select wave mode, set the clock to MCK / 2*/
   TC0->TC_CHANNEL[0].TC_CMR = TC_CMR_WAVSEL_UP_RC | TC_CMR_WAVE | TC_CMR_TCCLKS_TIMER_CLOCK1;
   /* Enable the RC Compare interrupt*/
diff --git a/due_obd2/statemachine.c b/due_obd2/statemachine.c
index a3a8ea5..23c7fe2 100644
--- a/due_obd2/statemachine.c
+++ b/due_obd2/statemachine.c
@@ -33,6 +33,11 @@ __UINT8_TYPE__ statemachine_init()
 
 void statemachine_loop()
 {
+    if (states[current_state_id].next == current_state_id)
+    {
+        next = 0;
+    }
+
     if (next)
     {
         next = 0;
@@ -41,13 +46,13 @@ void statemachine_loop()
         {
             states[current_state_id].on_exit();
         }
-        if (states[current_state_id].next >= 0 && states[current_state_id].next != current_state_id)
+        if (states[current_state_id].next >= 0)
         {
             current_state_id = states[current_state_id].next;
         }
         states[current_state_id].on_enter();
     }
-
+    
     if (states[current_state_id].on_run != NULL)
     {
         states[current_state_id].on_run();