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
}
