PCA9685版OTTO开源跳舞机器人
当初急用但直接在网上翻遍了也没找到PCA9685版的OTTO机器人,库是从M5stack找的。直接上全部源代码和当初的参考文献吧。需要请自取。
源代码网盘下载地址
提取码:8z71
或者https://codechina.csdn.net/happyjoey217/pca9685-otto/-/tree/master
正文
https://www.ottodiy.com/Otto 机器人的官网。下面的图是官网截图。
这个可爱的小家伙是一个简单的四自由度机器人(两个踝关节和两个髋关节),外壳是3D打印的,官网提供了全部的3D打印文件和源代码。还有论坛。可以让你以非常低廉的成本实现对四自由度机器人的探索。我还曾经把小OTTO倒过用它的一条腿做过云台实验。哈哈哈 上面粘了一个摄像头。有想像没有约束,在没有3D打印机的情况下,也曾用废旧的包装壳实现过。
教程地址
后来我自己在做其他小制作的时候发现了PCA9685,就想着用这个岂不更好用?而且可以外接电源支持更多的舵机。用更少的控制口。也许可以用WemosD1Mini来替代之前的设计。(好吧这是另外一篇帖子的故事了。。。敬请期待)但是在网上找了很久并没有见到有人做过这个尝试(至少2020年5月的时候)。于是就有了这篇帖子。
关于OTTO开源跳舞机器人
src是PCA9685的库文件所在的文件夹,nanoPca9685ODance.ino是主程序。
PCA9685库文件
下面两个文件放在src文件下。
Oscillater.cpp文件:
//--------------------------------------------------------------
//-- Oscillator.pde
//-- Generate sinusoidal oscillations in the servos
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------
#include "Oscillator.h"
Oscillator::Oscillator(void) {
}
Oscillator::Oscillator(Adafruit_PWMServoDriver* pwm, int ch, int servomin, int servomax, int trim) {
this->_pwm = pwm;
this->_ch = ch;
this->_servomin = servomin;
this->_servomax = servomax;
this->_trim = trim;
attach();
}
//-- This function returns true if another sample
//-- should be taken (i.e. the TS time has passed since
//-- the last sample was taken
bool Oscillator::next_sample()
{
//-- Read current time
_currentMillis = millis();
//-- Check if the timeout has passed
if(_currentMillis - _previousMillis > _TS) {
_previousMillis = _currentMillis;
return true;
}
return false;
}
//-- Attach an oscillator to a servo
//-- Input: pin is the arduino pin were the servo
//-- is connected
void Oscillator::attach(bool rev)
{
//-- Attach the servo and move it to the home position
// servo_write(90);
//-- Initialization of oscilaltor parameters
_TS=30;
_T=2000;
_NN = _T/_TS;
_inc = 2*M_PI/_NN;
_previousMillis=0;
//-- Default parameters
_A=45;
_phase=0;
_phase0=0;
_O=0;
_stop=false;
//-- Reverse mode
_rev = rev;
}
//-- Detach an oscillator from his servo
void Oscillator::detach()
{
}
/*************************************/
/* Set the oscillator period, in ms */
/*************************************/
void Oscillator::setT(unsigned int T)
{
//-- Assign the new period
_T=T;
//-- Recalculate the parameters
_NN = _T/_TS;
_inc = 2*M_PI/_NN;
};
/*******************************/
/* Manual set of the position */
/******************************/
void Oscillator::setPosition(int position)
{
servo_write(position);
};
/*******************************************************************/
/* This function should be periodically called */
/* in order to maintain the oscillations. It calculates */
/* if another sample should be taken and position the servo if so */
/*******************************************************************/
void Oscillator::refresh()
{
//-- Only When TS milliseconds have passed, the new sample is obtained
if (next_sample()) {
//-- If the oscillator is not stopped, calculate the servo position
if (!_stop) {
//-- Sample the sine function and set the servo pos
_pos = round(_A * sin(_phase + _phase0) + _O);
if (_rev) _pos=-_pos;
servo_write(_pos+90);
}
//-- Increment the phase
//-- It is always increased, even when the oscillator is stop
//-- so that the coordination is always kept
_phase = _phase + _inc;
}
}
void Oscillator::servo_write(int ang) {
ang = ang + _trim;
ang = map(ang, 0 , 180 , _servomin, _servomax);
_pwm->setPWM(_ch, 0, ang);
}
Oscillator.h文件
//--------------------------------------------------------------
//-- Oscillator.pde
//-- Generate sinusoidal oscillations in the servos
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------
#include "Oscillator.h"
Oscillator::Oscillator(void) {
}
Oscillator::Oscillator(Adafruit_PWMServoDriver* pwm, int ch, int servomin, int servomax, int trim) {
this->_pwm = pwm;
this->_ch = ch;
this->_servomin = servomin;
this->_servomax = servomax;
this->_trim = trim;
attach();
}
//-- This function returns true if another sample
//-- should be taken (i.e. the TS time has passed since
//-- the last sample was taken
bool Oscillator::next_sample()
{
//-- Read current time
_currentMillis = millis();
//-- Check if the timeout has passed
if(_currentMillis - _previousMillis > _TS) {
_previousMillis = _currentMillis;
return true;
}
return false;
}
//-- Attach an oscillator to a servo
//-- Input: pin is the arduino pin were the servo
//-- is connected
void Oscillator::attach(bool rev)
{
//-- Attach the servo and move it to the home position
// servo_write(90);
//-- Initialization of oscilaltor parameters
_TS=30;
_T=2000;
_NN = _T/_TS;
_inc = 2*M_PI/_NN;
_previousMillis=0;
//-- Default parameters
_A=45;
_phase=0;
_phase0=0;
_O=0;
_stop=false;
//-- Reverse mode
_rev = rev;
}
//-- Detach an oscillator from his servo
void Oscillator::detach()
{
}
/*************************************/
/* Set the oscillator period, in ms */
/*************************************/
void Oscillator::setT(unsigned int T)
{
//-- Assign the new period
_T=T;
//-- Recalculate the parameters
_NN = _T/_TS;
_inc = 2*M_PI/_NN;
};
/*******************************/
/* Manual set of the position */
/******************************/
void Oscillator::setPosition(int position)
{
servo_write(position);
};
/*******************************************************************/
/* This function should be periodically called */
/* in order to maintain the oscillations. It calculates */
/* if another sample should be taken and position the servo if so */
/*******************************************************************/
void Oscillator::refresh()
{
//-- Only When TS milliseconds have passed, the new sample is obtained
if (next_sample()) {
//-- If the oscillator is not stopped, calculate the servo position
if (!_stop) {
//-- Sample the sine function and set the servo pos
_pos = round(_A * sin(_phase + _phase0) + _O);
if (_rev) _pos=-_pos;
servo_write(_pos+90);
}
//-- Increment the phase
//-- It is always increased, even when the oscillator is stop
//-- so that the coordination is always kept
_phase = _phase + _inc;
}
}
void Oscillator::servo_write(int ang) {
ang = ang + _trim;
ang = map(ang, 0 , 180 , _servomin, _servomax);
_pwm->setPWM(_ch, 0, ang);
}
主程序:nanoPca9685ODance.ino
#include "src/Oscillator/Oscillator.h"
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
//---------------------------------
// Servo Settings 舵机设定
//---------------------------------
#define N_SERVOS 5 //五自由度舵机 0左脚 1右脚 2左腿 3右腿 4头
#define INTERVALTIME 10.0 //这是中断时间为10.0
int t = 620; // 1280; //脉冲宽度?
double pausemillis = 0;
int servoMin[N_SERVOS] = {150, 150, 150, 150, 150}; //舵机的最小输出
int servoMax[N_SERVOS] = {500, 500, 500, 500, 500}; //舵机的最大输出
int servoTrim[N_SERVOS] = {10, 0, 0, -10, -10}; //
int servoInit[N_SERVOS] = {90, 90, 90, 90, 125}; //舵机初始化
int servoTest[N_SERVOS] = {80, 80, 80, 80, 115}; //舵机测试
#define NECKV_CH 4
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
Oscillator *servo[N_SERVOS];
bool maintenanceMode = true;
void setup() {
Serial.begin(9600);
Serial.println("5 channel Servo test!");
Wire.begin();
pwm.begin();//启动舵机
pwm.setPWMFreq(50); //模拟伺服在60赫兹更新下运行 此点很重要
//-----------舵机初始化---------------------------
for (int i = 0; i < N_SERVOS; i ++) {
servo[i] = new Oscillator(&pwm, i, servoMin[i], servoMax[i], servoTrim[i]);
}
Serial.println("Servo init");
for (int i = 0; i < N_SERVOS; i++) {
servo[i]->setPosition(servoTest[i]);
}
delay(1000);
for (int i = 0; i < N_SERVOS; i++) {
servo[i]->setPosition(servoInit[i]);
}
//---------整机初始化完成,舵机休眠-----------
}
void loop() {
action1();
}
//------------编舞1------------------------
void action1() {
displayStatus("Walk");
walk(2, t * 3);
displayStatus("Back");
backyard(2, t * 3);
displayStatus("upDown");
upDown(3, t * 2);
displayStatus("MWalkR");
moonWalkRight(1, t*2);
moonWalkRight(2, t);
displayStatus("MWalkL");
moonWalkLeft(1, t*2);
moonWalkLeft(2, t);
displayStatus("swing");
swing(2, t);
}
//---------------输出显示---------------------
void displayStatus(String param) {
// Serial.print(param + " ");
}
//-------oscillate函数----------------------
void oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]) {
for (int i = 0; i < N_SERVOS; i++) {
servo[i]->setO(O[i]);
servo[i]->setA(A[i]);
servo[i]->setT(T);
servo[i]->setPh(phase_diff[i]);
}
double ref = millis();
for (double x = ref; x < T + ref; x = millis()) {
for (int i = 0; i < N_SERVOS; i++) {
servo[i]->refresh();
}
}
for (int i=0; i< N_SERVOS; i++) {
servo[i]->reset();
servo[i]->setPosition(90);
}
}
//-------------------moveNServos函数------------------------
unsigned long final_time;
unsigned long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS];
int oldPosition[N_SERVOS] = {90, 90, 90, 90, 90};
void moveNServos(int time, int newPosition[]) {
for (int i = 0; i < N_SERVOS; i++) increment[i] = ((newPosition[i]) - oldPosition[i]) / (time / INTERVALTIME);
final_time = millis() + time;
iteration = 1;
while (millis() < final_time) { //Javi del futuro cambia esto
interval_time = millis() + INTERVALTIME;
oneTime = 0;
while (millis() < interval_time) {
if (oneTime < 1) {
for (int i = 0; i < N_SERVOS; i++) {
servo[i]->setPosition(oldPosition[i] + (iteration * increment[i]));
}
iteration++;
oneTime++;
}
}
}
for (int i = 0; i < N_SERVOS; i++) {
oldPosition[i] = newPosition[i];
}
}
//-------------------------------------------------
// OTTO机器人的基本动作
//-------------------------------------------------
void walk(int steps, int T) {
int A[N_SERVOS] = { 15, 15, 30, 30, 25};
int O[N_SERVOS] = { 0, 0, 0, 0, 0};
double phase_diff[N_SERVOS] = { DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90), DEG2RAD(135) };
for (int i = 0; i < steps; i++) {
oscillate(A, O, T, phase_diff);
}
}
void backyard(int steps, int T) {
int A[N_SERVOS] = {15, 15, 30, 30, 25};
int O[N_SERVOS] = {0, 0, 0, 0, 0};
double phase_diff[N_SERVOS] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90), DEG2RAD(135)};
for (int i = 0; i < steps; i++) oscillate(A, O, T, phase_diff);
}
void upDown(int steps, int tempo) {
int move1[N_SERVOS] = {50, 130, 90, 90, 115};
int move2[N_SERVOS] = {90, 90, 90, 90, 90};
for (int x = 0; x < steps; x++) {
pausemillis = millis();
moveNServos(tempo * 0.2, move1);
delay(tempo * 0.4);
moveNServos(tempo * 0.2, move2);
while (millis() < (pausemillis + tempo));
}
}
void moonWalkRight(int steps, int T) {
Serial.println("moonWalkRight");
int A[N_SERVOS] = {15, 15, 0, 0, 0};
int O[N_SERVOS] = {5 , 5, 0, 0, 0};
double phase_diff[N_SERVOS] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90), DEG2RAD(90)};
for (int i = 0; i < steps; i++) oscillate(A, O, T, phase_diff);
}
void moonWalkLeft(int steps, int T) {
Serial.println("moonWalkLeft");
int A[N_SERVOS] = {15, 15, 0, 0, 0};
int O[N_SERVOS] = {5, 5, 0, 0, 0};
double phase_diff[6] = {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90), DEG2RAD(90)};
for (int i = 0; i < steps; i++) oscillate(A, O, T, phase_diff);
}
void swing(int steps, int T) {
int A[N_SERVOS] = {25, 25, 0, 0, 0};
int O[N_SERVOS] = {15, 15, 0, 0, 0};
double phase_diff[N_SERVOS] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90), DEG2RAD(90)};
for (int i = 0; i < steps; i++) oscillate(A, O, T, phase_diff);
}
void headSwing(int steps, int tempo) {
int move1[N_SERVOS] = {90, 90, 90, 90, 90};
int move2[N_SERVOS] = {90, 90, 90, 90, 45};
for (int x = 0; x < steps; x++) {
pausemillis = millis();
moveNServos(tempo * 0.2, move1);
delay(tempo * 0.4);
moveNServos(tempo * 0.2, move2);
while (millis() < (pausemillis + tempo));
}
}
void headSwing2(int steps, int T) {
int A[N_SERVOS] = {0, 0, 0, 0, 45};
int O[N_SERVOS] = {0, 0, 0, 0, 0};
double phase_diff[N_SERVOS] = {DEG2RAD(90), DEG2RAD(90), DEG2RAD(90), DEG2RAD(90), DEG2RAD(135) };
for (int i = 0; i < steps; i++) oscillate(A, O, T, phase_diff);
}
参考文献
【1】https://github.com/mongonta0716/M5Stack-OttoDIY/tree/master/M5Stack-OttoDIY/src/Oscillator
【2】视频演示:https://www.youtube.com/watch?v=y8X9X10Tn1k
https://steemit.com/utopian-io/@drencolha/adafruit-pca9685-pwm-servo-driver-setup-arduino-library-use-shown-with-an-example-project-tutorial
【3】连接图:https://steemit.com/utopian-io/@drencolha/adafruit-pca9685-pwm-servo-driver-setup-arduino-library-use-shown-with-an-example-project-tutorial