add sw project (incomplete)

RC excavator/FW/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

RC excavator/FW/Mini_Excavator_Code.bak

@@ -0,0 +1,369 @@
+#include "Adafruit_MCP23X17.h"
+#include <Ps3Controller.h>
+#include <ESP32Servo.h>  // by Kevin Harrington
+
+// defines
+#define clawServoPin 5
+#define auxServoPin 18
+#define cabLights 32
+#define auxLights 33
+
+#define pivot0 15
+#define pivot1 14
+#define mainBoom0 9
+#define mainBoom1 8
+#define secondBoom0 0
+#define secondBoom1 1
+#define tiltAttach0 3
+#define tiltAttach1 2
+#define thumb0 11
+#define thumb1 10
+#define auxAttach0 12
+#define auxAttach1 13
+
+#define leftMotor0 7
+#define leftMotor1 6
+#define rightMotor0 4
+#define rightMotor1 5
+
+
+Adafruit_MCP23X17 mcp;
+Servo clawServo;
+Servo auxServo;
+int dly = 250;
+int clawServoValue = 90;
+int auxServoValue = 90;
+int player = 0;
+int battery = 0;
+int servoDelay = 0;
+
+bool cabLightsOn = false;
+bool auxLightsOn = false;
+bool moveClawServoUp = false;
+bool moveClawServoDown = false;
+bool moveAuxServoUp = false;
+bool moveAuxServoDown = false;
+
+void notify() {
+  //--------------- Digital D-pad button events --------------
+  if (Ps3.event.button_down.up) {
+
+    mcp.digitalWrite(thumb0, HIGH);
+    mcp.digitalWrite(thumb1, LOW);
+    Serial.println("Started pressing the up button");
+  }
+  if (Ps3.event.button_up.up) {
+
+    mcp.digitalWrite(thumb0, LOW);
+    mcp.digitalWrite(thumb1, LOW);
+    Serial.println("Released the up button");
+  }
+  if (Ps3.event.button_down.down) {
+    Serial.println("Started pressing the down button");
+    mcp.digitalWrite(thumb0, LOW);
+    mcp.digitalWrite(thumb1, HIGH);
+  }
+  if (Ps3.event.button_up.down) {
+    Serial.println("Released the down button");
+    mcp.digitalWrite(thumb0, LOW);
+    mcp.digitalWrite(thumb1, LOW);
+  }
+  if (Ps3.event.button_down.right) {
+    mcp.digitalWrite(auxAttach0, HIGH);
+    mcp.digitalWrite(auxAttach1, LOW);
+    Serial.println("Started pressing the right button");
+  }
+  if (Ps3.event.button_up.right) {
+    mcp.digitalWrite(auxAttach0, LOW);
+    mcp.digitalWrite(auxAttach1, LOW);
+    Serial.println("Released the right button");
+  }
+  if (Ps3.event.button_down.left) {
+    mcp.digitalWrite(auxAttach0, LOW);
+    mcp.digitalWrite(auxAttach1, HIGH);
+    Serial.println("Started pressing the left button");
+  }
+  if (Ps3.event.button_up.left) {
+    mcp.digitalWrite(auxAttach0, LOW);
+    mcp.digitalWrite(auxAttach1, LOW);
+    Serial.println("Released the left button");
+  }
+  //---------------- Analog stick value events ---------------
+  if (abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2) {
+    Serial.print("Moved the left stick:");
+    Serial.print(" x=");
+    Serial.print(Ps3.data.analog.stick.lx, DEC);
+    Serial.print(" y=");
+    Serial.print(Ps3.data.analog.stick.ly, DEC);
+    Serial.println();
+    int LXValue = Ps3.data.analog.stick.lx;
+    Serial.print("LXValue =");
+    Serial.print(LXValue);
+    if (LXValue > 115) {
+      mcp.digitalWrite(pivot0, HIGH);
+      mcp.digitalWrite(pivot1, LOW);
+      delay(10);
+      Serial.print("Made to into Positive");
+    }
+    if (LXValue < -115) {
+      mcp.digitalWrite(pivot0, LOW);
+      mcp.digitalWrite(pivot1, HIGH);
+      delay(10);
+      Serial.print("Made to into negative");
+    }
+
+    int LYValue = Ps3.data.analog.stick.ly;
+    if (LYValue > 115) {
+      mcp.digitalWrite(secondBoom0, HIGH);
+      mcp.digitalWrite(secondBoom1, LOW);
+      delay(10);
+    }
+    if (LYValue < -115) {
+      mcp.digitalWrite(secondBoom0, LOW);
+      mcp.digitalWrite(secondBoom1, HIGH);
+      delay(10);
+    }
+    if (LXValue > -30 && LXValue < 30) {
+      mcp.digitalWrite(pivot0, LOW);
+      mcp.digitalWrite(pivot1, LOW);
+    }
+    if (LYValue > -30 && LYValue < 30) {
+      mcp.digitalWrite(secondBoom0, LOW);
+      mcp.digitalWrite(secondBoom1, LOW);
+    }
+  }
+
+  if (abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2) {
+    Serial.print("Moved the right stick:");
+    Serial.print(" x=");
+    Serial.print(Ps3.data.analog.stick.rx, DEC);
+    Serial.print(" y=");
+    Serial.print(Ps3.data.analog.stick.ry, DEC);
+    Serial.println();
+    int RXValue = (Ps3.data.analog.stick.rx);
+    if (RXValue > 115) {
+      mcp.digitalWrite(tiltAttach0, HIGH);
+      mcp.digitalWrite(tiltAttach1, LOW);
+      delay(10);
+      Serial.print("Made to into Positive");
+    }
+    if (RXValue < -115) {
+      mcp.digitalWrite(tiltAttach0, LOW);
+      mcp.digitalWrite(tiltAttach1, HIGH);
+      delay(10);
+      Serial.print("Made to into negative");
+    }
+
+    int RYValue = (Ps3.data.analog.stick.ry);
+    if (RYValue > 115) {
+      mcp.digitalWrite(mainBoom0, HIGH);
+      mcp.digitalWrite(mainBoom1, LOW);
+      delay(10);
+    }
+    if (RYValue < -115) {
+      mcp.digitalWrite(mainBoom0, LOW);
+      mcp.digitalWrite(mainBoom1, HIGH);
+      delay(10);
+    }
+    if (RXValue > -30 && RXValue < 30) {
+      mcp.digitalWrite(tiltAttach0, LOW);
+      mcp.digitalWrite(tiltAttach1, LOW);
+    }
+    if (RYValue > -30 && RYValue < 30) {
+      mcp.digitalWrite(mainBoom0, LOW);
+      mcp.digitalWrite(mainBoom1, LOW);
+    }
+  }
+  //------------- Digital shoulder button events -------------
+  if (Ps3.event.button_down.l1) {
+    mcp.digitalWrite(leftMotor0, HIGH);
+    mcp.digitalWrite(leftMotor1, LOW);
+    delay(10);
+    Serial.println("Started pressing the left shoulder button");
+  }
+  if (Ps3.event.button_up.l1) {
+    mcp.digitalWrite(leftMotor0, LOW);
+    mcp.digitalWrite(leftMotor1, LOW);
+    delay(10);
+    Serial.println("Released the left shoulder button");
+  }
+  if (Ps3.event.button_down.r1) {
+    mcp.digitalWrite(rightMotor0, HIGH);
+    mcp.digitalWrite(rightMotor1, LOW);
+    delay(10);
+    Serial.println("Started pressing the right shoulder button");
+  }
+  if (Ps3.event.button_up.r1) {
+    mcp.digitalWrite(rightMotor0, LOW);
+    mcp.digitalWrite(rightMotor1, LOW);
+    delay(10);
+    Serial.println("Released the right shoulder button");
+  }
+  //-------------- Digital trigger button events -------------
+  if (Ps3.event.button_down.l2) {
+    mcp.digitalWrite(leftMotor0, LOW);
+    mcp.digitalWrite(leftMotor1, HIGH);
+    delay(10);
+    Serial.println("Started pressing the left trigger button");
+  }
+  if (Ps3.event.button_up.l2) {
+    mcp.digitalWrite(leftMotor0, LOW);
+    mcp.digitalWrite(leftMotor1, LOW);
+    delay(10);
+    Serial.println("Released the left trigger button");
+  }
+  if (Ps3.event.button_down.r2) {
+    mcp.digitalWrite(rightMotor0, LOW);
+    mcp.digitalWrite(rightMotor1, HIGH);
+    delay(10);
+    Serial.println("Started pressing the right trigger button");
+  }
+  if (Ps3.event.button_up.r2) {
+    mcp.digitalWrite(rightMotor0, LOW);
+    mcp.digitalWrite(rightMotor1, LOW);
+    delay(10);
+    Serial.println("Released the right trigger button");
+  }
+  //--- Digital cross/square/triangle/circle button events ---
+  if (Ps3.event.button_down.cross) {
+    moveClawServoUp = true;
+    Serial.println("Started pressing the cross button");
+  }
+  if (Ps3.event.button_up.cross) {
+    moveClawServoUp = false;
+    Serial.println("Released the cross button");
+  }
+  if (Ps3.event.button_down.square) {
+    moveAuxServoUp = true;
+    Serial.println("Started pressing the square button");
+  }
+  if (Ps3.event.button_up.square) {
+    moveAuxServoUp = false;
+    Serial.println("Released the square button");
+  }
+  if (Ps3.event.button_down.triangle) {
+    moveClawServoDown = true;
+    Serial.println("Started pressing the triangle button");
+  }
+  if (Ps3.event.button_up.triangle) {
+    moveClawServoDown = false;
+  }
+
+  if (Ps3.event.button_down.circle) {
+    moveAuxServoDown = true;
+    Serial.println("Started pressing the circle button");
+  }
+  if (Ps3.event.button_up.circle) {
+    moveAuxServoDown = false;
+    Serial.println("Released the circle button");
+  }
+  //--------------- Digital stick button events --------------
+  if (Ps3.event.button_down.l3) {
+    if (!cabLightsOn) {
+      digitalWrite(cabLights, HIGH);
+      cabLightsOn = true;
+    } else {
+      digitalWrite(cabLights, LOW);
+      cabLightsOn = false;
+    }
+    Serial.println("Started pressing the left stick button");
+  }
+  if (Ps3.event.button_up.l3)
+    Serial.println("Released the left stick button");
+
+  if (Ps3.event.button_down.r3) {
+    if (!auxLightsOn) {
+      digitalWrite(auxLights, HIGH);
+      auxLightsOn = true;
+    } else {
+      digitalWrite(auxLights, LOW);
+      auxLightsOn = false;
+    }
+    Serial.println("Started pressing the right stick button");
+  }
+  if (Ps3.event.button_up.r3) {
+    Serial.println("Released the right stick button");
+  }
+  if (moveClawServoUp) {
+    if (servoDelay == 3) {
+      if (clawServoValue >= 10 && clawServoValue < 170) {
+        clawServoValue = clawServoValue + 1;
+        clawServo.write(clawServoValue);
+      }
+      servoDelay = 0;
+    }
+    servoDelay++;
+  }
+  if (moveClawServoDown) {
+    if (servoDelay == 3) {
+      if (clawServoValue <= 170 && clawServoValue > 10) {
+        clawServoValue = clawServoValue - 1;
+        clawServo.write(clawServoValue);
+      }
+      servoDelay = 0;
+    }
+    servoDelay++;
+  }
+  if (moveAuxServoUp) {
+    if (servoDelay == 3) {
+      if (auxServoValue >= 10 && auxServoValue < 170) {
+        auxServoValue = auxServoValue + 1;
+        auxServo.write(auxServoValue);
+      }
+      servoDelay = 0;
+    }
+    servoDelay++;
+  }
+  if (moveAuxServoDown) {
+    if (servoDelay == 3) {
+      if (auxServoValue <= 170 && auxServoValue > 10) {
+        auxServoValue = auxServoValue - 1;
+        auxServo.write(auxServoValue);
+      }
+      servoDelay = 0;
+    }
+    servoDelay++;
+  }
+}
+
+void onConnect() {
+  Serial.println("Connected.");
+}
+
+void setup() {
+
+  Serial.begin(115200);
+
+  mcp.begin_I2C();
+  //   put your setup code here, to run once:
+
+
+  for (int i = 0; i <= 15; i++) {
+    mcp.pinMode(i, OUTPUT);
+  }
+
+  Ps3.attach(notify);
+  Ps3.attachOnConnect(onConnect);
+  Ps3.begin("a0:5a:5a:a0:0f:91");
+
+  Serial.println("Ready.");
+
+  pinMode(clawServoPin, OUTPUT);
+  pinMode(auxServoPin, OUTPUT);
+
+  pinMode(cabLights, OUTPUT);
+  pinMode(auxLights, OUTPUT);
+
+  clawServo.attach(clawServoPin);
+  auxServo.attach(auxServoPin);
+  clawServo.write(clawServoValue);
+  auxServo.write(auxServoValue);
+}
+
+
+
+void loop() {
+  if (!Ps3.isConnected())
+    return;
+  delay(500);
+}

RC excavator/FW/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

RC excavator/FW/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

RC excavator/FW/platformio.ini

@@ -0,0 +1,19 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32dev]
+platform = espressif32
+board = esp32dev
+framework = arduino
+monitor_speed = 115200
+lib_deps =
+	asukiaaa/XboxSeriesXControllerESP32_asukiaaa @ ^1.0.9
+  adafruit/Adafruit MCP23017 Arduino Library @ ^2.3.2
+  madhephaestus/ESP32Servo @ ^3.0.5

RC excavator/FW/src/board.h

@@ -0,0 +1,25 @@
+#pragma once
+
+// defines
+#define clawServoPin 5
+#define auxServoPin 18
+#define cabLights 32
+#define auxLights 33
+
+#define pivot0 15
+#define pivot1 14
+#define mainBoom0 9
+#define mainBoom1 8
+#define secondBoom0 0
+#define secondBoom1 1
+#define tiltAttach0 3
+#define tiltAttach1 2
+#define thumb0 11
+#define thumb1 10
+#define auxAttach0 12
+#define auxAttach1 13
+
+#define leftMotor0 7
+#define leftMotor1 6
+#define rightMotor0 4
+#define rightMotor1 5

RC excavator/FW/src/controller.cpp

@@ -0,0 +1,65 @@
+#include "controller.h"
+
+XboxSeriesXControllerESP32_asukiaaa::Core xboxController; //"40:8e:2c:41:e4:eb"); //("a4:c1:38:38:e3:c0");
+uint32_t lastcontrollernotify = 0;
+
+
+
+void setup_controller()
+{
+  xboxController.begin();
+  xboxController.onLoop();
+  Serial.printf("Waiting for controller");
+}
+
+void loop_controller()
+{
+
+    xboxController.onLoop();
+    if (xboxController.isConnected())
+    {
+      if (xboxController.isWaitingForFirstNotification())
+      {
+        Serial.println("waiting for first notification");
+      }
+      else
+      {
+        log_v("Address: %s", xboxController.buildDeviceAddressStr());
+        // log_i("message: %s", Serial.print(xboxController.xboxNotif.toString()));
+        unsigned long receivedAt = xboxController.getReceiveNotificationAt();
+        uint16_t joystickMax = XboxControllerNotificationParser::maxJoy;
+        if (millis() - lastcontrollernotify > CONTROLLERNOTIFY)
+        {
+          log_i("joyLHori rate: %0.0f", ((float)xboxController.xboxNotif.joyLHori / joystickMax *100));
+          log_i("joyLVert rate: %0.0f",((float)xboxController.xboxNotif.joyLVert / joystickMax *100));
+          log_i("joyRHori rate: %0.0f", ((float)xboxController.xboxNotif.joyRHori / joystickMax *100));
+          log_i("joyRVert rate: %0.0f",((float)xboxController.xboxNotif.joyRVert / joystickMax *100));
+          log_i("battery %d %", xboxController.battery);
+          log_i("received at %l", receivedAt);
+          lastcontrollernotify = millis();
+        }
+        handle_notify(((float)xboxController.xboxNotif.joyLVert / joystickMax *100),
+                      ((float)xboxController.xboxNotif.joyRVert / joystickMax *100), 
+                      ((float)xboxController.xboxNotif.joyLHori / joystickMax *100),
+                      ((float)xboxController.xboxNotif.joyRHori / joystickMax *100)
+                      );
+      }
+    }
+    else  
+    {
+      log_w("not connected");
+      if (xboxController.getCountFailedConnection() > 10)
+      {
+        log_e("restarting");
+        ESP.restart();
+      }
+    }
+}
+
+void handle_notify(int motR, int motL, int motArm, int bucket)
+{
+  drivemotor(0, motR);
+  drivemotor(1, motL);
+  drivemotor(2, motArm);
+  bucketTilt(bucket);
+}

RC excavator/FW/src/controller.h

@@ -0,0 +1,8 @@
+#include "Arduino.h"
+#include <XboxSeriesXControllerESP32_asukiaaa.hpp>
+
+#define CONTROLLERNOTIFY 1000
+
+void setup_controller();
+void loop_controller();
+void handle_notify(int motR, int motL, int motArm, int bucket);

RC excavator/FW/src/main.cpp

@@ -0,0 +1,38 @@
+
+#include "controller.h"
+#include "motors.h"
+
+
+
+
+void setup() {
+
+  Serial.begin(115200);
+
+  // Ps3.attach(notify);
+  // Ps3.attachOnConnect(onConnect);
+  // Ps3.begin("a0:5a:5a:a0:0f:91");
+
+  // Serial.println("Ready.");
+
+  // pinMode(clawServoPin, OUTPUT);
+  // pinMode(auxServoPin, OUTPUT);
+
+  // pinMode(cabLights, OUTPUT);
+  // pinMode(auxLights, OUTPUT);
+
+  // clawServo.attach(clawServoPin);
+  // auxServo.attach(auxServoPin);
+  // clawServo.write(clawServoValue);
+  // auxServo.write(auxServoValue);
+  setup_controller();
+}
+
+
+
+void loop() {
+  // if (!Ps3.isConnected())
+  //   return;
+  // delay(500);
+  loop_controller();
+}

RC excavator/FW/src/motors.cpp

@@ -0,0 +1,174 @@
+#include "motors.h"
+
+Adafruit_MCP23X17 mcp;
+
+
+Servo bucketServo;
+Servo auxServo;
+
+
+#define NUMMOTORS 8
+
+motorinit initlist[NUMMOTORS]
+{
+  {pivot0,      pivot1,       0,  "Pivot"},
+  {mainBoom0,   mainBoom1,    1,  "Boom"},
+  {secondBoom0, secondBoom1,  2,  "Dipper"},
+  {tiltAttach0, tiltAttach1,  3,  "Bucket"},
+  {thumb0,      thumb1,       4,  "Thumb"},
+  {auxAttach0,  auxAttach1,   5,  "Aux"},
+  {leftMotor0,  leftMotor1,   6,  "TrackLeft"},
+  {rightMotor0, rightMotor1,  7,  "TrackRight"}
+};
+
+std::vector<motor> motors;
+
+
+void setup_motors()
+{
+  for (int i = 0; i < NUMMOTORS; i++)
+  {
+    motor newmotor(initlist[i]);
+    motors.push_back(newmotor);
+    log_i("%s motor(%i) initialized", newmotor.getName(), newmotor.getID() );
+    delay(50);
+  }
+  
+  for (auto &&thismotor: motors)
+  {
+    thismotor.begin();
+  }
+  // bucketServo.attach(bucketServoPin);
+  // bucketServo.write(default_bucketTilt);
+  //auxServo.attach(auxServoPin);
+  //auxControl(default_auxControl);
+}
+
+void controlMotor(String name, direction dir)
+{
+  for(auto &&thismotor : motors)
+  {
+    if(thismotor.getName() == name)
+    {
+      thismotor.run(dir);
+    }
+  }
+}
+
+void motor::begin()
+{
+  mcp.pinMode(m_pin0, OUTPUT);
+  mcp.pinMode(m_pin1, OUTPUT);
+  run(direction::stop);
+}
+
+void motor::setIO(uint8_t in0, uint8_t in1)
+{
+    mcp.digitalWrite(m_pin0, in0);
+    mcp.digitalWrite(m_pin1, in1);
+}
+
+void motor::run(direction dir)
+{
+  if(current == dir) return;
+
+  switch(dir)
+  {
+    case direction::left:
+    {
+      setIO(HIGH, LOW);
+      break;
+    }
+    case direction::right:
+    {
+      setIO(LOW, HIGH);
+      break;
+    }
+    case direction::stop:
+    default:
+    {
+      setIO(LOW, LOW);
+      break;
+    }
+  }
+  current = dir;
+}
+
+
+//uint32_t lastmotorlog = 0;
+
+
+// void drivemotor(uint8_t num, int dirspeed)
+// {
+//   if (num <= motorPins.size())
+//   {
+//     if(dirspeed > 46 && dirspeed < 55) //deadband
+//     {
+//       motors[num].stop();
+//       log_v("stop");
+//       return;
+//     }
+//     else
+//     {
+//       uint8_t mapspeed = map(dirspeed, 0, 100, 100, -100);
+//       motors[num].move(mapspeed);
+//       motors[num].prevsetpoint = mapspeed;
+//       if (millis() - lastmotorlog > MOTORLOGTIME)
+//       {
+//         log_i("drive motor %u, speed %d%(%d)", num, dirspeed, mapspeed);
+//         lastmotorlog = millis();
+//       }
+//     }
+//   }
+//   else
+//   {
+//     log_e("invalid motor num %d", num);
+//   }
+// }
+
+// int prevBucketAngle = 0;
+// uint32_t lastbuckettimer = 0;
+// void bucketTilt(int bucketServoValue)
+// {
+//   if(millis()- lastbuckettimer > BUCKETTIMER)
+//   {
+//     log_v("bucket command received = %i", bucketServoValue);
+//     int servo = 0;
+//     if(bucketServoValue == 0)
+//     {
+//       log_v("bucket stop");
+//       return;
+//     }
+//     else if(bucketServoValue > 0)
+//     {
+//       servo = prevBucketAngle + SERVOSTEP;
+//     }
+//     else
+//     {
+//       servo = prevBucketAngle - SERVOSTEP;
+//     }
+
+//     if(servo > SERVOMAX) servo = SERVOMAX;
+//     if(servo < SERVOMIN) servo = SERVOMIN;
+
+//     bucketServo.write(servo);
+//     log_v("bucket (%i) angle=(%d)", bucketServoValue, servo);
+
+//     lastbuckettimer = millis();
+//     prevBucketAngle = servo;
+//   }
+// }
+
+// void AbsBucketTilt(int bucketServoValue)
+// {
+//   log_v("drive bucket servo, angle  %d", bucketServoValue);
+//   delay(5);
+//   bucketServo.write(bucketServoValue);
+// }
+
+// void auxControl(int auxServoValue)
+// {
+//   log_v("drive aux servo, angle  %d", auxServoValue);
+//   delay(5);
+//   auxServo.write(auxServoValue);
+// }

RC excavator/FW/src/motors.h

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <Arduino.h>
+#include <ESP32Servo.h>
+#include <vector>
+#include "Adafruit_MCP23X17.h"
+#include "board.h"
+
+
+
+#define SERVOSTEP 10
+#define SERVOMIN  50
+#define SERVOMAX 180
+
+#define BUCKETTIMER 10
+
+#define MOTORLOGTIME 1000
+
+
+//general functions
+void setup_motors();
+void controlMotor(String name, direction dir);
+
+struct motorinit
+{
+  uint8_t in0;
+  uint8_t in1;
+  uint8_t id;
+  String name;
+};
+
+enum direction
+{
+  left,
+  right,
+  stop
+}
+
+
+class motor
+{
+  private:
+    uint8_t m_pin0;
+    uint8_t m_pin1;
+    uint8_t m_id;
+    String m_name;
+    uint32_t m_lastlog;
+    direction current;
+    void setIO(uint8_t in0, uint8_t in1);
+
+  public:
+    motor(motorinit initstruct):  m_pin0(initstruct.in0), 
+                                  m_pin1(initstruct.in1), 
+                                  m_id(initstruct.id), 
+                                  m_name(initstruct.name) {};
+   
+    void begin();
+    void run(direction dir);
+    void stop(); 
+    String getName(void) 
+    { 
+      return m_name;
+    }
+    uint8_t getID(void)
+    {
+      return m_id;
+    }
+
+};

RC excavator/FW/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html