8000 PCA9685 servo PWM driver test code · col-in-coding/robot-modeling@649b062 · GitHub
[go: up one dir, main page]
Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 649b062

Browse files
col-in-codingcol-in-coding
committed
PCA9685 servo PWM driver test code
1 parent 4c11beb commit 649b062

File tree

3 files changed

+250
-23
lines changed

3 files changed

+250
-23
lines changed

‎hardware_test/Quadruped/Quadruped.ino

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
#include <Wire.h>
2+
#include <Adafruit_PWMServoDriver.h>
3+
4+
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
5+
const uint8_t SERVO_PWM_FREQ {50}; // 50 Hz
6+
7+
void setup() {
8+
Serial.begin(9600);
9+
Serial.println("Robot Start!");
10+
Serial.println("Please select from the menu: ");
11+
Serial.println("1. Robot stand");
12+
Serial.println("#############################");
13+
14+
pwm.begin();
15+
delay(50);
16+
pwm.setPWMFreq(SERVO_PWM_FREQ);
17+
reset_robot();
18+
}
19+
20+
void set_servo_pulse(uint8_t n, double pulse) {
21+
double pulselength;
22+
23+
pulselength = 1000000; // 1,000,000 us per second
24+
pulselength /= SERVO_PWM_FREQ;
25+
// Serial.print(pulselength); Serial.println(" us per period");
26+
pulselength /= 4096; // 12 bits of resolution
27+
// Serial.print(pulselength); Serial.println(" us per bit");
28+
pulse *= 1000;
29+
pulse /= pulselength;
30+
// Serial.println(pulse);
31+
pwm.setPWM(n, 0, pulse);
32+
}
33+
34+
void write_servo_ang(uint8_t n,uint8_t angle){
35+
double pulse;
36+
pulse=0.5+angle/90.0;
37+
set_servo_pulse(n,pulse);
38+
}
39+
40+
41+
/***
42+
* Params:
43+
* alpha: Rate of convergence
44+
* mu: control the amplitude of the output signals, Ah = sqrt(mu)
45+
* Ah: Amplitude of the hip joint control signal
46+
* Ak: Amplitude of the knee joint control signal
47+
* beta: Duty cycle of the support phase (Load Factor)
48+
* omega_st / omega_sw = (1 - beta) / beta
49+
* omega_sw: Frequency of the swing phase
50+
* omega_st: Frequency of the support phase
51+
* a: rate of the change between omega_sw and omega_st
52+
* u: (optional, default 0), feedback, EX: u1 <=> x1, u2<=> y2...
53+
*
54+
* Outputs:
55+
* x1 => LF
56+
* x2 => RF
57+
* x3 => LH
58+
* x4 => RH
59+
*
60+
* x, the output of the oscillator, is control signal for hip joint
61+
* the Ascending part is in the swing phase
62+
* the Descending part is in support phase (when the knee joint is frozen)
63+
*/
64+
65+
/*
66+
const uint16_t SERVOMIN {102};
67+
const uint16_t SERVOMAX {512};
68+
uint8_t alpha {100};
69+
double Ah {0.2};
70+
double Ak {0.1};
71+
uint8_t a {10};
72+
double omega_sw {3 * M_PI};
73+
74+
// servo init angle with model coordinate
75+
double servo_int_ang[12] {
76+
45, 100, 180, 135, 90, 0, 135, 90, 0, 45, 90, 180
77+
};
78+
int8_t servo_dir[12] {
79+
1, -1, -1, -1, 1, 1, -1, -1, 1, 1, 1, -1
80+
};
81+
82+
// hip0, hip1, knee
83+
double servo_num[12] {
84+
0, 1, 2,
85+
3, 4, 5,
86+
6, 7, 8,
87+
9, 10, 11
88+
};
89+
// LF, RF, RH, LF
90+
// walk 0, 0.5, 0.25, 0.75
91+
// trot 0, 0.5, 0, 0.5
92+
double phi[4] {0, 0.5, 0.25, 0.75};
93+
double beta {0.5};
94+
95+
// current angle rad buffer
96+
double current_rad[12] {};
97+
*/
98+
const uint16_t SERVOMIN {102};
99+
const uint16_t SERVOMAX {512};
100+
101+
// servo init angle with model coordinate
102+
double servo_int_ang[12] {
103+
45, 100, 180, 135, 90, 0, 135, 90, 0, 45, 90, 180
104+
};
105+
106+
int8_t servo_dir[12] {
107+
1, -1, -1, -1, 1, 1, -1, -1, 1, 1, 1, -1
108+
};
109+
110+
// hip0, hip1, knee
111+
double servo_num[12] {
112+
0, 1, 2,
113+
3, 4, 5,
114+
6, 7, 8,
115+
9, 10, 11
116+
};
117+
118+
void reset_robot(){
119+
pwm.setPWM(0, 0, 0);
120+
pwm.setPWM(1, 0, 0);
121+
pwm.setPWM(2, 0, 0);
122+
pwm.setPWM(3, 0, 0);
123+
pwm.setPWM(4, 0, 0);
124+
pwm.setPWM(5, 0, 0);
125+
pwm.setPWM(6, 0, 0);
126+
}
127+
128+
// init STAND state
129+
void init_robot() {
130+
Serial.println("init robot");
131+
132+
write_servo_ang(0, 45);
133+
write_servo_ang(1, 100);
134+
write_servo_ang(2, 180);
135+
136+
write_servo_ang(4, 135);
137+
write_servo_ang(5, 90);
138+
write_servo_ang(6, 0);
139+
140+
// write_servo_ang(8, 135);
141+
// write_servo_ang(9, 90);
142+
// write_servo_ang(10, 0);
143+
}
144+
145+
String input_str = "";
146+
void loop() {
147+
if (Serial.available() > 0) {
148+
if (Serial.peek() != '\n') {
149+
input_str += (char)Serial.read();
150+
} else {
151+
Serial.read();
152+
Serial.print("Instruction received: ");
153+
Serial.println(input_str);
154+
switch (input_str.toInt()) {
155+
case 1:
156+
init_robot();
157+
break;
158+
case 2:
159+
reset_robot();
160+
break;
161+
}
162+
input_str = "";
163+
}
164+
delay(100);
165+
}
166+
}

‎hardware_test/motorTest/motorTest.ino

Lines changed: 40 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,37 +1,54 @@
1-
#include <Adafruit_PWMServoDriver.h>
21

3-
#define SERVOMIN 110
4-
#define SERVOMAX 512
2+
#include <Wire.h>
3+
#include "Adafruit_PWMServoDriver.h"
54

6-
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
5+
// called this way, it uses the default address 0x40
6+
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
77

8-
int convert2pulse(int ang) {
9-
int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX);
10-
return pulse;
11-
}
8+
// our servo # counter
9+
uint8_t servonum = 5;
1210

1311
void setup() {
14-
1512
Serial.begin(9600);
16-
Serial.println("Servo test! please input angle: ");
13+
Serial.println("16 channel Servo test!");
1714

1815
pwm.begin();
19-
pwm.setPWMFreq(50);
20-
delay(50);
16+
pwm.setPWMFreq(50); // Analog servos run at ~60 Hz updates
17+
}
18+
19+
// you can use this function if you'd like to set the pulse length in seconds
20+
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
21+
void setServoPulse(uint8_t n, double pulse) {
22+
double pulselength;
23+
24+
pulselength = 1000000; // 1,000,000 us per second
25+
pulselength /= 50; // 50 Hz
26+
Serial.print(pulselength); Serial.println(" us per period");
27+
pulselength /= 4096; // 12 bits of resolution
28+
Serial.print(pulselength); Serial.println(" us per bit");
29+
pulse *= 1000;
30+
pulse /= pulselength;
31+
Serial.println(pulse);
32+
pwm.setPWM(n, 0, pulse);
2133
}
2234

35+
void writeServo(uint8_t n,uint8_t angle){
36+
double pulse;
37+
pulse=0.5+angle/90.0;
38+
setServoPulse(n,pulse);
39+
}
40+
String inString = "";
2341
void loop() {
24-
String inString = "";
25-
while (Serial.available() > 0) {
26-
char inChar = Serial.read();
27-
if (inChar != "\n") {
28-
inString += (char)inChar;
29-
}
30-
delay(10);
31-
}
32-
if (inString != "") {
33-
Serial.print("I received: ");
42+
43+
if (Serial.available() > 0) {
44+
if (Serial.peek() != '\n') {
45+
inString += (char)Serial.read();
46+
} else {
47+
Serial.read();
48+
Serial.print("Instruction received: ");
3449
Serial.println(inString);
35-
pwm.setPWM(0, 0, convert2pulse(inString.toInt()));
50+
writeServo(servonum,inString.toInt());
51+
inString = "";
52+
}
3653
}
3754
}

‎hardware_test/motorsTest/motorsTest.ino

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
#include <Wire.h>
2+
#include "Adafruit_PWMServoDriver.h"
3+
4+
// called this way, it uses the default address 0x40
5+
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
6+
7+
void setServoPulse(uint8_t n, double pulse) {
8+
double pulselength;
9+
10+
pulselength = 1000000; // 1,000,000 us per second
11+
pulselength /= 50; // 50 Hz
12+
Serial.print(pulselength); Serial.println(" us per period");
13+
pulselength /= 4096; // 12 bits of resolution
14+
Serial.print(pulselength); Serial.println(" us per bit");
15+
pulse *= 1000;
16+
pulse /= pulselength;
17+
Serial.println(pulse);
18+
pwm.setPWM(n, 0, pulse);
19+
}
20+
21+
void writeServo(uint8_t n,uint8_t angle){
22+
double pulse;
23+
pulse=0.5+angle/90.0;
24+
setServoPulse(n,pulse);
25+
}
26+
27+
void setup() {
28+
Serial.begin(9600);
29+
Serial.println("16 channel PWM test!");
30+
pwm.begin();
31+
pwm.setPWMFreq(50); // This is the maximum PWM frequency
32+
33+
writeServo(0, 45);
34+
writeServo(1, 100);
35+
writeServo(2, 180);
36+
writeServo(3, 135);
37+
writeServo(4, 90);
38+
writeServo(5, 0);
39+
}
40+
41+
void loop() {
42+
43+
44+
}

0 commit comments

Comments
 (0)
0