Hi all!
I'm trying to create a project to control some (11) dc motors with BTS7960, and PCA9685 to generate PWM signal, both in speed and direction.
In the sketch I wrote a FSM with ramp function that calculate the speed.
If the speed is positive I'll write the 12bit equivalent to FW channel, if negative to BW channel of the BTS7960 (I'm not arrived to PCA9685 part of the sketch yet).
I faced the problem that if I use more than 1 motor, I'll get some spike in the calculated speed.
The sketch I wrote:
#include <Adafruit_PWMServoDriver.h>
#define NUM_MOTORS 3
#define MAX_NUM_STEPS 10
#define rampTime 1000
byte m = 0;
int out_SPEED;
enum States { STOP,
FIRST_RAMP,
RAMP,
SPEED,
REBOOT
};
unsigned long startTime;
byte pwmValue;
//MOTORS
typedef struct {
byte dim;
byte index;
const char *nome;
States state;
unsigned long timeStep;
int vel[MAX_NUM_STEPS];
unsigned long tempo[MAX_NUM_STEPS];
} Motors;
Motors motors[] = {
{ 2, 0, "M1", STOP, 0, { 25, -25 }, { 2000, 2000 } },
{ 2, 0, "M2", STOP, 0, { 50, -50 }, { 2000, 2000 } },
{ 2, 0, "M3", STOP, 0, { 75, -75 }, { 2000, 2000 } },
{ 2, 0, "M4", STOP, 0, { 100, -100 }, { 2000, 2000 } },
{ 2, 0, "M5", STOP, 0, { 10, 0 }, { 1000, 1000 } },
{ 2, 0, "M6", STOP, 0, { 50, -50 }, { 2000, 2000 } },
{ 2, 0, "M7", STOP, 0, { 75, -75 }, { 2000, 2000 } },
{ 2, 0, "M8", STOP, 0, { 100, -100 }, { 2000, 2000 } },
{ 2, 0, "M9", STOP, 0, { 75, -75 }, { 2000, 2000 } },
{ 2, 0, "M10", STOP, 0, { 100, -100 }, { 2000, 2000 } },
{ 2, 0, "M11", STOP, 0, { 100, -100 }, { 2000, 2000 } }
};
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
while (m < NUM_MOTORS) {
pwmValue = updateMotor(motors[m], m);
Serial.print(",");
m += 1;
};
Serial.println();
m = 0;
}
//function to interpolate speed based on machine state
byte updateMotor(Motors &motor, byte num) {
//linear interpolation
//y = mx + q
byte index = motor.index;
byte dim = motor.dim;
if (index >= motor.dim) {
motor.state = REBOOT;
}
//unsigned long now = millis();
unsigned long elapsedTime = millis() - motor.timeStep;
switch (motor.state) {
case STOP:
motor.timeStep = millis();
out_SPEED = 0;
motor.state = FIRST_RAMP;
break;
case FIRST_RAMP:
if (elapsedTime < rampTime) {
out_SPEED = (motor.vel[motor.index] / float(rampTime)) * float(elapsedTime);
} else {
motor.timeStep = millis();
motor.state = SPEED;
}
break;
case RAMP:
if (elapsedTime < rampTime) {
out_SPEED = ((motor.vel[motor.index] - motor.vel[motor.index - 1]) / float(rampTime)) * float(elapsedTime) + motor.vel[motor.index - 1];
} else {
motor.timeStep = millis();
motor.state = SPEED;
}
break;
case SPEED:
if (elapsedTime < motor.tempo[motor.index]) {
out_SPEED = motor.vel[motor.index];
} else {
motor.timeStep = millis();
motor.index += 1;
motor.state = RAMP;
}
break;
case REBOOT:
if (elapsedTime < rampTime) {
out_SPEED = ((motor.vel[0] - motor.vel[motor.dim - 1]) / float(rampTime)) * float(elapsedTime) + motor.vel[motor.dim - 1];
} else {
motor.index = 0;
motor.timeStep = millis();
motor.state = SPEED;
}
}
switch (num) {
case 0:
Serial.print("M1:");
Serial.print(out_SPEED);
Serial.print(",");
Serial.print("index1:");
Serial.print(motor.index);
break;
case 1:
Serial.print("M2:");
Serial.print(out_SPEED);
break;
case 2:
Serial.print("M3:");
Serial.print(out_SPEED);
break;
case 3:
Serial.print("M4:");
Serial.print(out_SPEED);
break;
case 4:
Serial.print("M5:");
Serial.print(out_SPEED);
break;
case 5:
Serial.print("M6:");
Serial.print(out_SPEED);
break;
}
return out_SPEED;
}
Using serial plotter (with only 3 motors) I get this output
Maybe I'm blind but I can't figure out where the spike come from.
And the problem is that this spike will send to motor a voltage spike...
Any advice?
Thanks in advance
Stefano
