working dipper boom, pivot

RC excavator/FW/platformio.ini

@@ -17,3 +17,7 @@ lib_deps =
 	asukiaaa/XboxSeriesXControllerESP32_asukiaaa @ ^1.0.9
   adafruit/Adafruit MCP23017 Arduino Library @ ^2.3.2
   madhephaestus/ESP32Servo @ ^3.0.5
+build_flags = 
+	-DCORE_DEBUG_LEVEL=3
+	-DNDEF_DEBUG=1
+	-fexceptions

RC excavator/FW/src/controller.cpp

@@ -3,13 +3,23 @@
 XboxSeriesXControllerESP32_asukiaaa::Core xboxController; //"40:8e:2c:41:e4:eb"); //("a4:c1:38:38:e3:c0");
 uint32_t lastcontrollernotify = 0;
 
-int joymin = 0;
+uint16_t joymax = 0;
-int joymax = 0;
+#define deadband 10000
-
 
 direction getdirection(int setpoint)
 {
-  if(setpoint > )
+  if(setpoint > (joymax/2 +deadband))
+  {
+    return direction::right;
+  }
+  else if(setpoint < (joymax/2 -deadband))
+  {
+    return direction::left;
+  }
+  else
+  {
+    return direction::stop;
+  }
   
 }
 
@@ -17,6 +27,8 @@ void setup_controller()
 {
   xboxController.begin();
   xboxController.onLoop();
+  xboxController.startScan();
+
   Serial.printf("Waiting for controller");
 }
 
@@ -34,23 +46,34 @@ void loop_controller()
         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;
+        joymax = XboxControllerNotificationParser::maxJoy;
         if (millis() - lastcontrollernotify > CONTROLLERNOTIFY)
         {
-          log_i("joyLHori rate: %0.0f", ((float)xboxController.xboxNotif.joyLHori / joystickMax *100));
+          log_i("joyLHori rate: %u", xboxController.xboxNotif.joyLHori);
-          log_i("joyLVert rate: %0.0f",((float)xboxController.xboxNotif.joyLVert / joystickMax *100));
+          log_i("joyLVert rate: %u", xboxController.xboxNotif.joyLVert);
-          log_i("joyRHori rate: %0.0f", ((float)xboxController.xboxNotif.joyRHori / joystickMax *100));
+          log_i("joyRHori rate: %u", xboxController.xboxNotif.joyRHori);
-          log_i("joyRVert rate: %0.0f",((float)xboxController.xboxNotif.joyRVert / joystickMax *100));
+          log_i("joyRVert rate: %u", xboxController.xboxNotif.joyRVert);
           log_i("battery %d %", xboxController.battery);
-          log_i("received at %l", receivedAt);
+          log_i("joymax:%d",joymax );
           lastcontrollernotify = millis();
         }
-        controlMotor("mainboom", direction::stop)
+        controlMotor("Pivot", getdirection(xboxController.xboxNotif.joyLHori));
+        controlMotor("Dipper", getdirection(xboxController.xboxNotif.joyLVert));
+
+        controlMotor("Bucket", getdirection(xboxController.xboxNotif.joyRHori));
+        controlMotor("Boom", getdirection(xboxController.xboxNotif.joyRVert));
+
+        //controlMotor("TrackLeft", getdirection(xboxController.xboxNotif.trigLT)
+
       }
     }
     else  
+    {
+        if (millis() - lastcontrollernotify > CONTROLLERNOTIFY)
         {
           log_w("not connected");
+          lastcontrollernotify = millis();
+        }
       if (xboxController.getCountFailedConnection() > 10)
       {
         log_e("restarting");
@@ -58,11 +81,3 @@ void loop_controller()
       }
     }
 }
-
-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/main.cpp

@@ -9,22 +9,7 @@ void setup() {
 
   Serial.begin(115200);
 
-  // Ps3.attach(notify);
+  setup_motors();
-  // 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();
 }
 

RC excavator/FW/src/motors.cpp

@@ -26,6 +26,7 @@ std::vector<motor> motors;
 
 void setup_motors()
 {
+  mcp.begin_I2C();
   for (int i = 0; i < NUMMOTORS; i++)
   {
     motor newmotor(initlist[i]);
@@ -51,8 +52,10 @@ void controlMotor(String name, direction dir)
     if(thismotor.getName() == name)
     {
       thismotor.run(dir);
+      return;
     }
   }
+  log_e("motor %s not found",name);
 }
 
 void motor::begin()
@@ -68,6 +71,17 @@ void motor::setIO(uint8_t in0, uint8_t in1)
     mcp.digitalWrite(m_pin1, in1);
 }
 
+uint32_t lastmotorlog = 0;
+
+void logactions(String dir, String name)
+{
+  if (millis() - lastmotorlog > MOTORLOGTIME)
+  {
+    log_i("motor %s: %s", name, dir);
+    lastmotorlog = millis();
+  }
+}
+
 void motor::run(direction dir)
 {
   if(current == dir) return;
@@ -77,98 +91,23 @@ void motor::run(direction dir)
     case direction::left:
     {
       setIO(HIGH, LOW);
+      log_i("motor %s: left", getName());
       break;
     }
     case direction::right:
     {
       setIO(LOW, HIGH);
+      log_i("motor %s: Right", getName());
+
       break;
     }
     case direction::stop:
     default:
     {
       setIO(LOW, LOW);
+      log_i("motor %s: stop", getName());
       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

@@ -17,9 +17,7 @@
 #define MOTORLOGTIME 1000
 
 
-//general functions
+
-void setup_motors();
-void controlMotor(String name, direction dir);
 
 struct motorinit
 {
@@ -36,6 +34,9 @@ enum direction
   stop
 };
 
+//general functions
+void setup_motors();
+void controlMotor(String name, direction dir);
 
 class motor
 {