Tyler pit steppers

.gitignore

@@ -1,4 +1,5 @@
 .pio
+.idea/
 .vscode/*
 compile_flags.txt
 .clangd

include/BME280.hpp

@@ -4,9 +4,7 @@
 // This code is designed to work with the BME280_I2CS I2C Mini Module available from ControlEverything.com.
 // https://www.controleverything.com/content/Humidity?sku=BME280_I2CS#tabs-0-product_tabset-2
 
-#ifndef BME280_H
-#define BME280_H
-
+#pragma once
 #include <Wire.h>
 
 // BME280 I2C address is 0x76(108)
@@ -215,5 +213,3 @@ class BME280
     return humidity;
   }
 };
-
-#endif

include/IncrementalEncoder.hpp

@@ -1,5 +1,4 @@
-#ifndef ENCODER_DRIVER_H
-#define ENCODER_DRIVER_H
+#pragma once
 
 #include <Arduino.h>
 
@@ -96,6 +95,3 @@ class IncrementalEncoder
 
 IncrementalEncoder *IncrementalEncoder::encoders[16];
 int IncrementalEncoder::encoderCount = 0;
-
-
-#endif

include/IntervalTimers.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <Arduino.h>
+#include <IntervalTimer.h>
+#include <functional>
+
+// There are only 4 timers allowed, as described in the interval timer code
+const int NUM_TIMERS = 4;
+IntervalTimer timers[NUM_TIMERS];
+
+// Function pointer array which will be used for timer callbacks
+using TimerCallback = std::function<void()>;
+TimerCallback timerCallbacks[NUM_TIMERS] = {nullptr};
+
+/**
+ * @brief Attaches a callback function to a specific timer.
+ *
+ * This function associates a callback function with one of the available timers.
+ * The callback will be executed when the timer triggers.
+ *
+ * @param timerIndex The index of the timer to attach the callback to (0 to NUM_TIMERS-1).
+ * @param callback The function to be called when the timer triggers.
+ */
+void attachTimerCallback(int timerIndex, TimerCallback callback) {
+  if (timerIndex >= 0 && timerIndex < NUM_TIMERS) {
+	timerCallbacks[timerIndex] = callback;
+  }
+}
+
+/**
+ * @brief Sets the frequency for a specific timer.
+ *
+ * This function sets the frequency at which a timer will trigger its callback.
+ * If the frequency is set to 0, the timer will be stopped.
+ *
+ * @param timerIndex The index of the timer to set the frequency for (0 to NUM_TIMERS-1).
+ * @param frequency The desired frequency in Hz. Set to 0 to stop the timer.
+ */
+void setTimerFrequency(int timerIndex, uint32_t frequency) {
+  if (timerIndex >= 0 && timerIndex < NUM_TIMERS) {
+	if (frequency > 0) {
+	  uint32_t period_us = 1000000 / frequency;
+	  timers[timerIndex].begin(timerCallbacks[timerIndex], period_us);
+	} else {
+	  timers[timerIndex].end();
+	}
+  }
+}

include/PIDController.hpp

@@ -1,6 +1,4 @@
-#ifndef PIDCONTROLLER
-#define PIDCONTROLLER
-
+#pragma once
 #include <Arduino.h>
 
 class PIDController {
@@ -89,6 +87,4 @@ class PIDController {
     }
 
 
-};
-
-#endif // PIDCONTROLLER
+};

include/Sabertooth.hpp

@@ -1,6 +1,4 @@
-#ifndef SABERTOOTH2X25
-#define SABERTOOTH2X25
-
+#pragma once
 #include <Arduino.h>
 
 // Sabertooth2x25 motor driver. Operated in simplified serial mode
@@ -43,4 +41,3 @@ class Sabertooth {
         serial->write(motor << 7 | (int)lerp(speed, -255, 255, 1, 127));
     }
 };
-#endif

include/Stepper.hpp

@@ -1,81 +1,135 @@
-#ifndef STEPPER_DRIVER_H
-#define STEPPER_DRIVER_H
+#pragma once
 
 #include <Arduino.h>
+#include "IntervalTimers.hpp"
 
 #define DEFAULT_MAX_STEPPER_FREQ 1000
 #define DEFAULT_MIN_STEPPER_FREQ 100
 
 class Stepper {
-private:
-    int pul_pin;
-    int dir_pin;
-    int min_freq_hz;
-    int max_freq_hz;
-    int freq_hz = 0;
-    unsigned long period_us = 0;
-    float lerp(float x, float a1, float b1, float a2, float b2){return a2 + (x - a1) * (b2 - a2) / (b1 - a1);}
-    elapsedMicros pulseTimer;
-
-public:
-
-    /**
-     * Constructor for Stepper motor driver
-     * @param pul_pin The pin to use for the pulse signal
-     * @param dir_pin The pin to use for the direction signal
-     * @param min_freq The minimum frequency to use for the stepper motor. Default is 100 Hz
-     * @param max_freq The maximum frequency to use for the stepper motor. Default is 2000 Hz
-    */
-    Stepper(int pul_pin, int dir_pin, int min_freq = DEFAULT_MIN_STEPPER_FREQ, int max_freq = DEFAULT_MAX_STEPPER_FREQ) {
-
-        //https://forum.pjrc.com/index.php?threads/using-the-open-drain-capabilities-of-the-teensy-3-1-processor.25393/
-
-        this->pul_pin = pul_pin;
-        this->dir_pin = dir_pin;
-        this->min_freq_hz = min_freq;
-        this->max_freq_hz = max_freq;
-
-        pinMode(pul_pin, OUTPUT_OPENDRAIN);
-        pinMode(dir_pin, OUTPUT_OPENDRAIN);
-    }
-
-    /**
-     * Set the speed of the stepper motor
-     * @param speed The speed to set the motor to. -255 to 255
-    */
-    void setSpeed(int speed) {
-        int dir = speed < 0 ? 1 : 0;
-        speed = abs(constrain(speed, -255, 255));
-        digitalWriteFast(dir_pin, dir);
-        if (speed == 0) {
-            digitalWriteFast(pul_pin, LOW);
-            freq_hz = 0;
-            period_us = 0;
-            return;
-        }
-
-        freq_hz = (int) lerp(speed, 0, 255, min_freq_hz, max_freq_hz);
-        period_us = 100000 / freq_hz;
-
-        Serial.println("Speed: " + String(speed) + " Freq: " + String(freq_hz) + " Period: " + String(period_us));
-
-    }
-
-    /**
-     * Update the pulse signal for the stepper motor
-     * must be called at sufficient frequency. very minimum of 3-4 times the period of the signal per second
-    */
-    void updatePin() {
-        if (freq_hz == 0) {
-            return;
-        }
-
-        if (pulseTimer > period_us) {
-            pulseTimer = 0;
-            digitalToggleFast(pul_pin);
-        }
-
-    }
-};
-
-#endif // STEPPER_DRIVER_H
+ private:
+  int pul_pin;
+  int dir_pin;
+  int freq;
+  int min_freq_hz;
+  int max_freq_hz;
+  int timer_index;
+
+/**
+ * @brief Performs linear interpolation between two ranges.
+ *
+ * This function calculates the linear interpolation of a value x
+ * from one range (a1, b1) to another range (a2, b2).
+ *
+ * @param x The value to interpolate.
+ * @param a1 The lower bound of the first range.
+ * @param b1 The upper bound of the first range.
+ * @param a2 The lower bound of the second range.
+ * @param b2 The upper bound of the second range.
+ * @return The interpolated value in the second range.
+ *
+ * @note This function assumes that x is within the range [a1, b1].
+ *       No clamping is performed if x is outside this range.
+ */
+  float lerp(float x,
+			 float a1,
+			 float b1,
+			 float a2,
+			 float b2) { return a2 + (x - a1) * (b2 - a2) / (b1 - a1); }
+
+ public:
+  /**
+   * Constructor for Stepper motor driver
+   * @param pul_pin The pin to use for the pulse signal
+   * @param dir_pin The pin to use for the direction signal
+   * @param min_freq The minimum frequency to use for the stepper motor. Default is 100 Hz
+   * @param max_freq The maximum frequency to use for the stepper motor. Default is 2000 Hz
+  */
+  Stepper(int pul_pin,
+		  int dir_pin,
+		  int min_freq = DEFAULT_MIN_STEPPER_FREQ,
+		  int max_freq = DEFAULT_MAX_STEPPER_FREQ) {
+	this->pul_pin = pul_pin;
+	this->dir_pin = dir_pin;
+	this->freq = 0;
+	this->min_freq_hz = min_freq;
+	this->max_freq_hz = max_freq;
+
+	this->timer_index = -1;
+
+	// iterate to check if any of the timers are available
+	for (int i = 0; i < NUM_TIMERS; ++i) {
+	  // CASE: timer not in use (callback is still null)
+	  if (timerCallbacks[i] == nullptr) {
+		attachTimerCallback(i, [this]() { this->updatePin(); });
+		this->timer_index = i;
+		break;
+	  }
+	}
+
+	// CASE: there were no timers left, stepper was not properly initialized
+	if (this->timer_index == -1) {
+	  Serial.print("More than ");
+	  Serial.print(NUM_TIMERS);
+	  Serial.println(" PIT steppers, the newest one will not be active");
+	}
+
+	// set output pins to open drain configuration
+	pinMode(pul_pin,
+			OUTPUT_OPENDRAIN);
+	pinMode(dir_pin,
+			OUTPUT_OPENDRAIN);
+
+	// set speed
+	this->setSpeed(freq);
+  }
+
+  /**
+   * Set the speed of the stepper motor
+   * @param speed The speed to set the motor to. -255 to 255
+  */
+  void setSpeed(int speed) {
+	// get direction based on speed sign
+	int dir = speed < 0 ? 1 : 0;
+
+	// calculate magnitude speed
+	speed = abs(constrain(speed,
+						  -255,
+						  255));
+	// set direction pin
+	digitalWriteFast(this->dir_pin,
+					 dir);
+
+	// CASE: set to stop
+	if (speed == 0) {
+	  // set pin pulse low
+	  digitalWriteFast(this->pul_pin,
+					   LOW);
+	  // stop timer
+	  setTimerFrequency(this->timer_index,
+						0);
+	  return;
+	}
+
+	// update frequency
+	int freq_hz = (int) lerp(speed,
+							 0,
+							 255,
+							 this->min_freq_hz,
+							 this->max_freq_hz);
+	this->freq = freq_hz;
+	setTimerFrequency(this->timer_index,
+					  freq_hz);
+
+	int period_us = 1000000 / freq_hz;
+	Serial.println("Speed: " + String(speed) + " Freq: " + String(freq_hz) + " Period: " + String(period_us));
+  }
+
+  /**
+   * Update the pulse signal for the stepper motor
+   * called by an interrupt timer at the stepper frequency
+  */
+  void updatePin() {
+	digitalToggleFast(this->pul_pin);
+  }
+};

include/commands.h → include/commands.hpp

@@ -5,8 +5,7 @@
 // TODO fix inconsistent naming
 // TODO convert to enum
 
-#ifndef COMMANDS_H
-#define COMMANDS_H
+#pragma once
 
 enum COMMANDS
 {
@@ -40,5 +39,3 @@ enum COMMANDS
 
 #define READ_BATTERY_VOLTAGE 'g'
 #define BME_SENSOR 'b'
-
-#endif

include/pinout.h → include/pinout.hpp

@@ -1,5 +1,4 @@
-#ifndef PINOUT_H
-#define PINOUT_H
+#pragma once
 
 #include <Arduino.h>
 
@@ -43,6 +42,4 @@
 
 // ---------------- Misc Pin Assignments ----------------
 
-#define HEADLIGHT 9
-
-#endif
+#define HEADLIGHT 9

src/Main.cpp

@@ -1,7 +1,10 @@
 #include <Arduino.h>
 // macros
-#include "commands.h"
-#include "pinout.h"
+#include "commands.hpp"
+#include "pinout.hpp"
+
+// Timers
+#include "IntervalTimers.hpp"
 
 // Hardware drivers
 #include "Sabertooth.hpp"
@@ -245,15 +248,6 @@ void loop()
 
   parseSerial();
 
-  // need to update pins manually because we are using an open collecter (common anode) configuration which isnt supported by the hardware pwm timer.
-  // see https://www.omc-stepperonline.com/download/DM542T.pdf section 4 for more info on open drain configuration
-
-  // TODO manually updating pins work fine for now but will be severely impacted by any blocking code. need to look into a better solution
-  wristInclinationMotor->updatePin();
-  wristRotationMotor->updatePin();
-  baseMotor->updatePin();
-  gripperMotor->updatePin();
-
   // Safety auto stop
   // if (millis() - lastCmd > AUTO_STOP_INTERVAL) {
   //   lastCmd = millis();