#include <Wire.
h>
#include <Adafruit_PWMServoDriver.h>
int pulse_;
int sv_;
int last_pulse;
#define run_every(t) for (static uint16_t last_; \
(uint16_t)(uint16_t(millis()) - last_) >= (t); \
last_ += (t))
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
const byte NumServos = 6;
const int VelPeriod = 50; // how often the
servo position is recalculated
const int USMIN[NumServos] = { 150, 150, 150, 150, 150, 150 }; // minimum pulse
length (in uS)
const int USMAX[NumServos] = { 500, 600, 600, 600, 600, 600 }; // maximum pulse
length (in uS)
const int ServoPins[NumServos] = { 3, 5, 6, 9, 10, 11 };
const int InitialPosition[NumServos] = { // pulse len in microseconds
(USMAX[0] + USMIN[0]) / 2,
(USMAX[1] + USMIN[1]) / 2,
(USMAX[2] + USMIN[2]) / 2,
(USMAX[3] + USMIN[3]) / 2,
(USMAX[4] + USMIN[4]) / 2,
(USMAX[5] + USMIN[5]) / 2
};
bool AtTargets = true;
float sign(float a) {
if (a < 0) return -1;
if (a > 0) return +1;
return 0;
}
//-----------------------------------------------------------
// sqr()
// square funtion
// it's a function not a macro like sq()
//-----------------------------------------------------------
float sqr(float a) {
return a * a;
}
float target[NumServos]; // target position
float p[NumServos]; // current position
float v[NumServos]; // velocity: (change in p per step)
float vmax[NumServos]; // max change in p per step
float amax[NumServos]; // max change in v per step
byte group[NumServos]; // 0 = move independently; 1..n = move together
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
float co_eff = 0.97;
int pos[5];
int ppos[5];
float filter_pos[5];
int cvt_angle(int pulse_) {
pulse_ = constrain(pulse_, 0, 180);
return map(pulse_, 0, 180, SERVOMIN, SERVOMAX);
}
// our servo # counter
uint8_t servonum = 0;
void MoveServos() {
static unsigned long previousMillis = 0;
if (millis() - previousMillis < VelPeriod)
return;
previousMillis = millis();
float g[NumServos];
float v1, a1, tmax, sign1, p0;
for (byte grp = 0; grp < NumServos; grp++) {
tmax = 0;
for (byte servo = 0; servo < NumServos; servo++)
if (group[servo] == grp) {
g[servo] = abs(target[servo] - p[servo]) / vmax[servo];
if (amax[servo] > 0) {
sign1 = sign(sqr(v[servo]) + amax[servo] * 2 * (2 * abs(v[servo]) +
sign(v[servo]) * (p[servo] - target[servo])));
g[servo] = max(g[servo],
(float)(sign1 * abs(v[servo]) + sqrt(abs(2 * sign1 *
v[servo] * abs(v[servo]) + 4 * amax[servo] * (p[servo] - target[servo])))) /
amax[servo]);
}
tmax = max(tmax, g[servo]);
}
for (byte servo = 0; servo < NumServos; servo++)
if (group[servo] == grp) {
if (grp == 0)
tmax = g[servo];
if ((tmax < 2) or ((abs(v[servo]) <= amax[servo] + 0.0001) and
(abs(p[servo] + v[servo] - target[servo]) <= 1))) {
p[servo] = target[servo];
v[servo] = 0;
} else if (tmax > 0) {
g[servo] = g[servo] / tmax;
a1 = amax[servo] * sqr(g[servo]);
if (a1 == 0) {
v[servo] = (target[servo] - p[servo]) / tmax;
} else {
v1 = vmax[servo] * g[servo];
p0 = 2 * v[servo] + abs(v[servo]) * v[servo] / (2 * a1);
a1 = sign(target[servo] - p[servo] - p0) * a1;
if (a1 * (target[servo] - p[servo]) < 0) {
a1 = sign(a1) * abs(v[servo]) * v[servo] / (2 * (target[servo] -
p[servo]));
a1 = constrain(a1, -amax[servo], amax[servo]);
}
v[servo] = v[servo] + a1;
v[servo] = constrain(v[servo], -v1, v1);
}
v[servo] = constrain(v[servo], -vmax[servo], vmax[servo]);
p[servo] = p[servo] + v[servo];
}
}
}
for (byte servo = 0; servo < NumServos; servo++) {
p[servo] = constrain(p[servo], USMIN[servo], USMAX[servo]);
//myservo[servo].writeMicroseconds(p[servo]);
pwm.setPWM(servo, 0, p[servo]);
//Serial.println("servo[" + String(servo) + "]:" + String(p[servo]));
}
}
void setup() {
Serial.begin(9600);
Serial.println("4 channel Servo test!");
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
for (byte servo = 0; servo < NumServos; servo++) {
p[servo] = InitialPosition[servo];
v[servo] = 0;
target[servo] = p[servo];
amax[servo] = 0.1;
vmax[servo] = 10.0;
pwm.setPWM(servo, 0, p[servo]);
group[servo] = 0;
}
delay(10);
vmax[0] = 200;
amax[0] = 0;
group[0] = 1;
vmax[1] = 200;
amax[1] = 0;
group[1] = 1;
vmax[2] = 200;
amax[2] = 1.0;
group[2] = 0;
vmax[3] = 200;
amax[3] = 0;
group[3] = 0;
vmax[4] = 200;
amax[4] = 1.0;
group[4] = 1;
vmax[5] = 200;
amax[5] = 1.0;
group[5] = 1;
}
// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void SetTarg(byte servo, int pos, int tWait) {
target[servo] = pos;
if (tWait > 0) {
long u = millis();
while (millis() < u + tWait) {
MoveServos();
}
}
}
void loop() {
bool move_ = 0;
// Drive each servo one at a time using setPWM()
if (Serial.available()) {
String c = Serial.readString();
int ind = c.indexOf("s");
if (ind != -1) {
Serial.println("ind:" + String(ind) + String(c[ind + 1]));
int sv_ = c[ind + 1] - (char)('0');
int pulse_ = c.substring(ind + 2).toInt();
Serial.println("sv" + String(sv_) + " pos: " + String(pulse_));
pos[sv_] = pulse_;
move_ = 1;
//pwm.setPWM(sv_, 0, pulse_);
}
}
SetTarg[0, 400, 0];
SetTarg[1, 400, 0];
SetTarg[2, 400, 0];
SetTarg[3, 400, 0];
SetTarg[4, 400, 500];
//move_all_servo();
}
