Hi everyone,
I'm working on controlling the torque of a geared DC motor, but I'm currently stuck on calibrating my current sensor.
My encoder is accurately evaluating both position and direction. However, the current sensor data is unreliable. I've tried several methods to enhance and filter the output, but nothing has worked so far.
To help provide context, I've attached the following files:
-
Schematic: (Please note: I have changed a few pins from the original design - PWM2 = 9, DIR2 = 8, CHA = 3, CHB = 2). and I have used a 10K ohm resistance and Capacitor 104 (0.1 microfarad) with the analog output of the sensor.
-
Calibration Test File: My isolated code for testing the current sensor.
-
Trial Code: This code tracks the rotor in a sin wave form (works perfectly, except for the current readings).
-
Plots: Visuals obtained from the serial data.
Has anyone experienced similar issues or have any recommendations on standard practices to properly calibrate this sensor? Any advice or insights would be greatly appreciated.
Thanks in advance for your time and help!
Current Sensor Caliberation : (Ran this for about a 1.30 minutes)
#include <Arduino.h>
#include "CytronMotorDriver.h"
#include "ACS712.h"
#include <pwm.h>
PwmOut pwmPin(9);
CytronMD motor(PWM_DIR, 9, 8);
ACS712 acs(A0, 5.0, 8192, 185);
#define CPR 1428L
#define DEFAULT_SPEED 160
#define OVERSAMPLE_N 64
#define OVERSAMPLE_US 25 //32 -> 25
#define FILTER_ALPHA 0.25f
#define PRINT_EVERY_MS 50UL
volatile long encoderCounts = 0;
volatile uint8_t prevState = 0;
static const int8_t ENC_TABLE[16] = {
0, 1, -1, 0,
-1, 0, 0, 1,
1, 0, 0, -1,
0, -1, 1, 0
};
void doEncoder() {
uint8_t a = digitalRead(3);
uint8_t b = digitalRead(2);
uint8_t cur = (a << 1) | b;
encoderCounts += ENC_TABLE[(prevState << 2) | cur];
prevState = cur;
}
// ----------------------------------------
bool running = false;
float filteredI = 0.0f;
int currentSpd = DEFAULT_SPEED;
void driveMotor(int spd) { motor.setSpeed(spd); }
float readCurrentAmps() {
long sum = 0;
for (uint8_t i = 0; i < OVERSAMPLE_N; i++) {
sum += acs.mA_DC();
delayMicroseconds(OVERSAMPLE_US);
}
return (sum / (float)OVERSAMPLE_N) / 1000.0f;
}
void setup() {
Serial.begin(115200);
analogReadResolution(14);
pwmPin.begin(10000.0f, 5.0f);
pinMode(3, INPUT_PULLUP);
pinMode(2, INPUT_PULLUP);
uint8_t a = digitalRead(3);
uint8_t b = digitalRead(2);
prevState = (a << 1) | b;
attachInterrupt(digitalPinToInterrupt(3), doEncoder, CHANGE);
attachInterrupt(digitalPinToInterrupt(2), doEncoder, CHANGE);
Serial.println(F("# Calibrating — keep motor still..."));
acs.setMidPoint(8192);
long mpSum = 0;
for (uint8_t i = 0; i < 10; i++) {
acs.autoMidPoint();
mpSum += acs.getMidPoint();
delay(100);
}
acs.setMidPoint((int)(mpSum / 10));
Serial.print(F("# Midpoint = "));
Serial.println(acs.getMidPoint());
Serial.println(F("# Ready. Send START."));
}
void loop() {
static uint32_t lastPrint = 0;
static long lastCounts = 0;
static uint32_t lastRpmTime = 0;
static float rpm = 0.0f;
if (Serial.available()) {
String cmd = Serial.readStringUntil('\n');
cmd.trim();
cmd.toUpperCase();
if (cmd == "START") {
if (!running) {
running = true;
currentSpd = DEFAULT_SPEED;
noInterrupts(); encoderCounts = 0; interrupts();
lastCounts = 0;
rpm = 0.0f;
driveMotor(currentSpd);
filteredI = readCurrentAmps();
lastPrint = millis();
lastRpmTime = millis();
Serial.println(F("t_ms,spd_cmd,I_raw_A,I_filt_A,RPM"));
}
} else if (cmd == "STOP") {
running = false;
driveMotor(0);
Serial.println(F("# Stopped."));
} else if (running) {
int val = cmd.toInt();
if (val != 0 || cmd == "0") {
currentSpd = constrain(val, -255, 255);
driveMotor(currentSpd);
Serial.print(F("# Speed -> ")); Serial.println(currentSpd);
}
}
}
if (!running) return;
uint32_t now = millis();
if (now - lastPrint >= PRINT_EVERY_MS) {
uint32_t dt = now - lastRpmTime;
noInterrupts();
long cnt = encoderCounts;
interrupts();
if (dt > 0) {
long deltaCounts = cnt - lastCounts;
rpm = (deltaCounts / (float)CPR) * (60000.0f / dt);
lastCounts = cnt;
lastRpmTime = now;
}
float iRaw = readCurrentAmps();
filteredI += FILTER_ALPHA * (iRaw - filteredI);
Serial.print(now); Serial.print(',');
Serial.print(currentSpd); Serial.print(',');
Serial.print(iRaw, 4); Serial.print(',');
Serial.print(filteredI, 4); Serial.print(',');
Serial.println(rpm, 1);
lastPrint = now;
}
}
output :
Trail project : Sine wave Tracking
#include <Arduino.h>
#include "CytronMotorDriver.h"
#define CURRENT_PIN A0
#define SENSITIVITY 0.066f //0.185 ->
#define CPR 1428L
#define PRINT_MS 200UL
CytronMD motor(PWM_DIR, 9, 8);
inline void driveMotor(int spd) { motor.setSpeed(-spd); }
volatile long counts = 0;
volatile uint8_t prevState = 0;
static const int8_t ENC_TABLE[16] = {
0, 1, -1, 0,
-1, 0, 0, 1,
1, 0, 0, -1,
0, -1, 1, 0
};
void doEncoder() {
uint8_t a = digitalRead(3);
uint8_t b = digitalRead(2);
uint8_t cur = (a << 1) | b;
counts += ENC_TABLE[(prevState << 2) | cur];
prevState = cur;
}
float offsetV = 0.0f;
float filteredI = 0.0f;
void updateCurrent() {
float v = analogRead(CURRENT_PIN) * (5.0f / 16383.0f);
filteredI += 0.10f * ((v - offsetV) / SENSITIVITY - filteredI); //low pass filter with alpha = 0.01
}
const float kp = 6.0f;
const float kd = 0.5f; // 1.5 -> 0.5
const float kff = 0.0f; // 15 -> 0
const float amplitudeRads = 3.0f * PI;
const float periodSecs = 20.0f;
const float omega = TWO_PI / periodSecs;
const float N = 21.0f; // dirty-derivative cutoff (rad/s); tune 15–40 //15 -> 21
float lastError = 0.0f;
float posFilt = 0.0f;
float posFiltPrev = 0.0f;
bool isRunning = false;
unsigned long startTime = 0;
unsigned long lastTime = 0;
void setup() {
Serial.begin(115200);
analogReadResolution(14);
pinMode(3, INPUT_PULLUP);
pinMode(2, INPUT_PULLUP);
uint8_t a = digitalRead(3);
uint8_t b = digitalRead(2);
prevState = (a << 1) | b;
attachInterrupt(digitalPinToInterrupt(3), doEncoder, CHANGE);
attachInterrupt(digitalPinToInterrupt(2), doEncoder, CHANGE);
Serial.println(F("Calibrating..."));
long sum = 0;
for (int i = 0; i < 500; i++) { sum += analogRead(CURRENT_PIN); delay(2); }
offsetV = (sum / 500.0f) * (5.0f / 16383.0f);
Serial.print(F("OffsetV=")); Serial.println(offsetV, 4);
driveMotor(7);
Serial.println(F("Send START"));
}
void loop() {
static uint32_t lastPrint = 0;
unsigned long now = millis();
if (!isRunning) {
if (Serial.available()) {
String s = Serial.readStringUntil('\n');
s.trim(); s.toUpperCase();
if (s == F("START")) {
isRunning = true;
startTime = now;
lastTime = now;
lastPrint = now;
lastError = 0.0f;
posFilt = 0.0f;
posFiltPrev = 0.0f;
noInterrupts(); counts = 0; interrupts();
driveMotor(0);
Serial.println(F("t_ms,target_rad,actual_rad,targetW,omega_meas,pwm,I_filt"));
}
}
return;
}
if (now - lastTime >= 3UL) {
float dt = (now - lastTime) * 0.001f;
lastTime = now;
noInterrupts();
long cnt = counts;
interrupts();
updateCurrent();
float actualRad = (cnt / (float)CPR) * TWO_PI;
float t = (now - startTime) * 0.001f;
float targetRad = amplitudeRads * sinf(omega * t);
float targetW = amplitudeRads * omega * cosf(omega * t);
float error = targetRad - actualRad;
float deriv = (error - lastError) / dt;
lastError = error;
float alpha = N * dt / (1.0f + N * dt);
posFilt += alpha * (actualRad - posFilt);
float omegaMeas = (posFilt - posFiltPrev) / dt;
posFiltPrev = posFilt;
float vOut = kp * error + kd * deriv + kff * targetW;
int pwm = (int)constrain(vOut / 12.0f * 255.0f, -255.0f, 255.0f);
driveMotor(pwm);
if (now - lastPrint >= PRINT_MS) {
lastPrint = now;
Serial.print(now - startTime); Serial.print(',');
Serial.print(targetRad, 4); Serial.print(',');
Serial.print(actualRad, 4); Serial.print(',');
Serial.print(targetW, 4); Serial.print(',');
Serial.print(omegaMeas, 4); Serial.print(',');
Serial.print(pwm); Serial.print(',');
Serial.println(filteredI, 4);
}
if (t >= periodSecs * 2.0f) {
driveMotor(0);
Serial.println(F("DONE"));
isRunning = false;
}
}
}
Output
