From 41828b9a206f03eddc23892423d4db664153948b Mon Sep 17 00:00:00 2001
From: Willem Oldemans <willem@oldemans.nl>
Date: Wed, 8 Nov 2023 08:50:58 +0100
Subject: [PATCH] esp23 motor class

---
 FW/platformio.ini    |  2 +-
 FW/src/Motors.cpp    | 50 +++++++++++++++++++++-----------------------
 FW/src/config.h      |  2 +-
 FW/src/motors.h      |  7 ++++++-
 FW/src/webserver.cpp |  2 --
 5 files changed, 32 insertions(+), 31 deletions(-)

diff --git a/FW/platformio.ini b/FW/platformio.ini
index 449d323..3423f24 100644
--- a/FW/platformio.ini
+++ b/FW/platformio.ini
@@ -22,5 +22,5 @@ lib_deps =
 build_flags = 
 	-DCORE_DEBUG_LEVEL=3
 	-DNDEF_DEBUG=1
-upload_protocol = espota
+upload_protocol = espota ;esptool
 upload_port = MiniSkidi.local
diff --git a/FW/src/Motors.cpp b/FW/src/Motors.cpp
index 3fdd616..fa83cc3 100644
--- a/FW/src/Motors.cpp
+++ b/FW/src/Motors.cpp
@@ -5,46 +5,46 @@ bool removeArmMomentum = false;
 Servo bucketServo;
 Servo auxServo;
 
-
 void motor::drive(int dirspeed)
 {
-  if(dirspeed == 0)
+  if (dirspeed == 0)
   {
     motorStop(motorindex);
   }
-  if(dirspeed > 0)
+  if (dirspeed > 0)
   {
-    //map(value, fromLow, fromHigh, toLow, toHigh)
-    uint8_t speed = map(dirspeed,-255,0,0,100);
-    log_i("motor forward");
+    // map(value, fromLow, fromHigh, toLow, toHigh)
+    uint8_t speed = map(dirspeed, -255, 0, 0, 100);
+    log_i("motor(%u) forward", m_index);
     motorForward(motorindex, speed);
   }
   else
   {
-    uint8_t speed = map(dirspeed,0,255,0,100);
-    log_i("motor reverse");
+    uint8_t speed = map(dirspeed, 0, 255, 0, 100);
+    log_i("motor(%u) reverse", m_index);
     motorReverse(motorindex, speed);
   }
 }
 
-
-
-std::vector<MOTOR_PINS> motorPins = 
-{
-  {RMOTOR_IN1, RMOTOR_IN2},  //RIGHT_MOTOR Pins (IN1, IN2)
-  {LMOTOR_IN1, LMOTOR_IN2},  //LEFT_MOTOR  Pins
-//  {ARM_IN1,    ARM_IN2}, //ARM_MOTOR pins 
+std::vector<MOTOR_PINS> motorPins =
+    {
+        {RMOTOR_IN1, RMOTOR_IN2}, // RIGHT_MOTOR Pins (IN1, IN2)
+        {LMOTOR_IN1, LMOTOR_IN2}, // LEFT_MOTOR  Pins
+        //  {ARM_IN1,    ARM_IN2}, //ARM_MOTOR pins
 };
 
-motor motors[2];
+#define NUMMOTORS 2
+motor motors[NUMMOTORS];
 
 void setupmotors()
 {
-      
-  for (int i = 0; i < motorPins.size(); i++)
-  {  
-    motors[i].attachMotor(motorPins[i].pinIN1,motorPins[i].pinIN2);    // pinMode(motorPins[i].pinIN1, OUTPUT);
+
+  for (int i = 0; i < NUMMOTORS; i++)
+  {
+    log_i("motor(%u) initialized", motors[i].m_index);
+    motors[i].attachMotor(motorPins[i].pinIN1, motorPins[i].pinIN2); // pinMode(motorPins[i].pinIN1, OUTPUT);
     motors[i].motorStop(0);
+    motors[i].setindex(i);
   }
   bucketServo.attach(bucketServoPin);
   auxServo.attach(auxServoPin);
@@ -54,20 +54,18 @@ void setupmotors()
 
 void drivemotor(uint8_t num, int dirspeed)
 {
-  if(num < motorPins.size())
+  if (num < motorPins.size())
   {
+    log_i("drive motor %u, speed %d", num, dirspeed);
     motors[num].drive(dirspeed);
   }
 }
 
-
-
 void bucketTilt(int bucketServoValue)
 {
-  bucketServo.write(bucketServoValue); 
+  bucketServo.write(bucketServoValue);
 }
 void auxControl(int auxServoValue)
 {
-  auxServo.write(auxServoValue); 
+  auxServo.write(auxServoValue);
 }
-
diff --git a/FW/src/config.h b/FW/src/config.h
index d5d54b0..55c4f52 100644
--- a/FW/src/config.h
+++ b/FW/src/config.h
@@ -15,7 +15,7 @@
 //defaults
 
 #define default_auxControl  150
-#define default_bucketTilt  100
+#define default_bucketTilt  200
 
 
 
diff --git a/FW/src/motors.h b/FW/src/motors.h
index f726c2e..6400d75 100644
--- a/FW/src/motors.h
+++ b/FW/src/motors.h
@@ -49,7 +49,12 @@ void auxControl(int auxServoValue);
 class motor: public ESP32MotorControl
 {
   const uint8_t motorindex = 0;
-
   public:
+    uint8_t m_index;
+
   void drive(int dirspeed);
+  void setindex(uint8_t index)
+  {
+    m_index = index;
+  }
 };
\ No newline at end of file
diff --git a/FW/src/webserver.cpp b/FW/src/webserver.cpp
index 17130f8..f677a25 100644
--- a/FW/src/webserver.cpp
+++ b/FW/src/webserver.cpp
@@ -18,8 +18,6 @@ void handle_message(WebsocketsMessage msg) {
 
 void setup_webserver()
 {
-  Serial.begin(9600);
-
   // HTTP handler assignment
   webserver.on("/", HTTP_GET, [](AsyncWebServerRequest * request) {
     AsyncWebServerResponse *response = request->beginResponse_P(200, "text/html", web_smars_html_gz, web_smars_html_gz_len);