Scribd
Upload
46 views

Ramp Test

This document defines code for controlling 6 servos using an Adafruit PWMServoDriver. It initializes the PWM driver and defines constants for the minimum/maximum pulse widths and initial positions of each servo. Functions are defined to set the target position of individual servos, move all servos towards their targets, and constrain the pulse widths. In setup, the PWM driver is configured and initial servo positions are set. In loop, it checks for serial commands to move individual servos and calls the function to move all servos towards their targets.

Uploaded by

Thinh Hoang
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
46 views

Ramp Test

This document defines code for controlling 6 servos using an Adafruit PWMServoDriver. It initializes the PWM driver and defines constants for the minimum/maximum pulse widths and initial positions of each servo. Functions are defined to set the target position of individual servos, move all servos towards their targets, and constrain the pulse widths. In setup, the PWM driver is configured and initial servo positions are set. In loop, it checks for serial commands to move individual servos and calls the function to move all servos towards their targets.

Uploaded by

Thinh Hoang
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

#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();
}

You might also like

pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy