add sw project (incomplete)

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;
+    }
+
+};