This is the closest I got on a code which provides some sort of acceleration/deaceleration on jumping btw positions. I wonder if is there an official way to achieve this smoothly. Thanks
#include "unit_rolleri2c.hpp"
#include <M5Unified.h>
/*
Going to random position every 1 second - trying to move with ease in/out, but very artificial/jittery - not smooth.
*/
UnitRollerI2C RollerI2C;
const int32_t UNITS_PER_DEGREE = 100;
const int32_t POSITION_TOLERANCE = 150; // ~1.5° arrival threshold
const uint32_t MOVE_DURATION_MS = 3000; // total time per move (ms) — tune 1000–5000
const uint32_t STEP_MS = 40; // trajectory update interval (ms)
const uint32_t PAUSE_MS = 1000; // pause at target before next move
int32_t basePosition = 0;
// ── Ease-in / ease-out using a sine curve ──────────────────────────────────
// t=0.0 → 0.0 | t=0.5 → 0.5 | t=1.0 → 1.0
// Accelerates from rest, decelerates to rest — smooth S-curve.
float easeInOut(float t) {
return (1.0f - cosf(t * PI)) / 2.0f;
}
// Drives from startUnits → targetUnits over MOVE_DURATION_MS with ease in/out.
// Returns true when the motor confirms arrival within tolerance.
bool easedMoveTo(int32_t startUnits, int32_t targetUnits) {
uint32_t moveStart = millis();
while (true) {
uint32_t elapsed = millis() - moveStart;
float t = min((float)elapsed / (float)MOVE_DURATION_MS, 1.0f);
// Compute eased intermediate setpoint
float eased = easeInOut(t);
int32_t intermediate = startUnits + (int32_t)((targetUnits - startUnits) * eased);
RollerI2C.setPos(intermediate);
// Once we've sent the final setpoint, check arrival
if (t >= 1.0f) {
int32_t actual = RollerI2C.getPosReadback();
if (abs(targetUnits - actual) <= POSITION_TOLERANCE) {
return true; // ✅ arrived
}
// Give a small extra window for the PID to settle
if (elapsed > MOVE_DURATION_MS + 1000) {
return false; // ⏱️ timed out even after easing finished
}
}
delay(STEP_MS);
}
}
int32_t degreesToUnits(float deg) { return (int32_t)(deg * UNITS_PER_DEGREE); }
float unitsToDegrees(int32_t u) { return u / (float)UNITS_PER_DEGREE; }
void setup() {
M5.begin();
Serial.begin(115200);
delay(500);
randomSeed(analogRead(0));
bool found = RollerI2C.begin(&Wire, 0x64, 2, 1, 400000);
if (!found) {
Serial.println("❌ Roller485 NOT found on I2C! Check wiring.");
while (true) delay(1000);
}
Serial.println("✅ Roller485 found!");
Serial.print("Vin (x0.01V): "); Serial.println(RollerI2C.getVin());
Serial.print("ErrorCode: "); Serial.println(RollerI2C.getErrorCode());
RollerI2C.setOutput(0);
RollerI2C.setMode(ROLLER_MODE_POSITION);
RollerI2C.setPosMaxCurrent(80000);
RollerI2C.setOutput(1);
delay(100);
basePosition = RollerI2C.getPosReadback();
Serial.print("Base position (raw): "); Serial.println(basePosition);
Serial.println("Ready. Starting eased random position loop...\n");
delay(500);
}
int32_t currentTarget = 0; // tracks last target so we ease FROM it
void loop() {
// Random angle offset from base: -180° to +180°
float targetDegrees = random(-180, 181);
int32_t targetUnits = basePosition + degreesToUnits(targetDegrees);
int32_t fromUnits = RollerI2C.getPosReadback(); // start from actual current pos
Serial.print("➡️ ");
Serial.print(unitsToDegrees(fromUnits - basePosition), 1);
Serial.print("° → ");
Serial.print(targetDegrees, 1);
Serial.println("° (easing...)");
bool arrived = easedMoveTo(fromUnits, targetUnits);
float actualDeg = unitsToDegrees(RollerI2C.getPosReadback() - basePosition);
if (arrived) {
Serial.print("✅ Arrived at ");
} else {
Serial.print("⏱️ Stopped at ");
}
Serial.print(actualDeg, 1);
Serial.println("°");
Serial.print(" ErrorCode: "); Serial.println(RollerI2C.getErrorCode());
Serial.println();
delay(PAUSE_MS); // pause 1 second at target
}
