From f5d0707aa75abecaf1316f1ca8a6090c3cfa38e1 Mon Sep 17 00:00:00 2001
From: Kevin Harrington <mad.hephaestus@gmail.com>
Date: Wed, 20 Dec 2023 19:35:41 -0500
Subject: [PATCH] Switch to scan mode on startup to deal with multi networks

---
 .gitignore         |  1 +
 GetIMU.cpp         |  7 +++----
 GetIMU.h           |  2 +-
 MarcosFirmware.ino | 13 ++++++++++---
 ServoServer.cpp    |  1 +
 5 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/.gitignore b/.gitignore
index f925d37..6feb766 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@
 *.settings*
 /Release/
 /sloeber.ino.cpp
+/spec.d
diff --git a/GetIMU.cpp b/GetIMU.cpp
index d4e2cd6..a76fc70 100644
--- a/GetIMU.cpp
+++ b/GetIMU.cpp
@@ -15,15 +15,14 @@ void GetIMU::event(float * buffer) {
   bufferINTERNAL[9]=bno->orientationY;
   bufferINTERNAL[10]=bno->orientationX;
   bufferINTERNAL[11]=bno->orientationZ;
-  
+
 
 	for (int i = 0; i < NUM_IMU_VALUES; i++) {
 		buffer[i] = bufferINTERNAL[i];
 	}
-
-}
-bool GetIMU::loop() {
+  //print();
 }
+
 void GetIMU::setXPosition(float x){
 	bufferINTERNAL[12] =x;
 }
diff --git a/GetIMU.h b/GetIMU.h
index 2a205aa..656b232 100644
--- a/GetIMU.h
+++ b/GetIMU.h
@@ -33,7 +33,7 @@ class GetIMU: public PacketEventAbstract {
 	// Buffer contains data from the packet cming in at the start of the function
 	// User data is written into the buffer to send it back
 	void event(float * buffer);
-	bool loop();
+
 	void startSensor(EasyBNO055_ESP * _bno);
 	void print();
 
diff --git a/MarcosFirmware.ino b/MarcosFirmware.ino
index f4ff1f1..9ad4cc7 100644
--- a/MarcosFirmware.ino
+++ b/MarcosFirmware.ino
@@ -34,19 +34,26 @@ void otherI2CUpdate(){
 }
 
 void setup() {
-  manager.setup();
+  manager.setupScan();
+  manager.startThread();
+  servos = new ServoServer();
+  while(manager.getState()!=Connected){
+    vTaskDelay(10); //sleep 10ms
+  }
+  Serial.println("Starting Firmware Setup after WiFi connection");
   sensor = new GetIMU();
 	bno.start(&otherI2CUpdate);
   sensor->startSensor(&bno);
   Serial.println("Loading with name: " + name[0]);
-  servos = new ServoServer();
+
   coms.attach(new NameCheckerServer(name));
   coms.attach((PacketEvent*)sensor);
   coms.attach((PacketEvent*)servos);
+  Serial.println("Starting Firmware Loop");
+
 }
 
 void loop() {
-  manager.loop();
   if(manager.getState()==Connected)
     coms.server();
 }
diff --git a/ServoServer.cpp b/ServoServer.cpp
index a55d027..cb572f0 100644
--- a/ServoServer.cpp
+++ b/ServoServer.cpp
@@ -70,4 +70,5 @@ void ServoServer::event(float *buffer) {
     //if(value>0&& value<254)
     	servos[i]->write(constrain(value, 0, 180));
   }
+
 }