#include <Servo.h>
#include <Oscillator.h>
#include <US.h>
#include <Otto.h>
#define N_SERVOS 6 // 共6个舵机
#define PIN_YL 2 // 左腿
#define PIN_YR 3 // 右腿
#define PIN_RL 4 // 左脚
#define PIN_RR 5 // 右脚
#define PIN_AL 6 // 左胳膊
#define PIN_AR 7 // 右胳膊
#define INTERVALTIME 10.0
Oscillator servo[N_SERVOS];
void setup()
{
Serial.begin(9600);
// 设置舵机引脚
servo[0].attach(PIN_RR);
servo[1].attach(PIN_RL);
servo[2].attach(PIN_YR);
servo[3].attach(PIN_YL);
servo[4].attach(PIN_AR); //右胳膊 90-平直、0-向上
servo[5].attach(PIN_AL); //左胳膊 90-平直、180-向上
for(int i=0;i<N_SERVOS;i++) // 舵机归位
{
servo[i].SetPosition(90);
}
}
void loop()
{
walkTest(1, 500);
}
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();
}
}
}
void walkTest(int steps, int T){
int A[6]= {15, 15, 30, 30, 30, 30}; // 移动幅度 (引脚对应分别是:右脚 左脚 右腿 左腿 右胳膊 左胳膊)
int O[6] = {0, 0, 0, 0, 0, 0}; // 偏移 (+偏右 -偏左)
double phase_diff[6] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90), DEG2RAD(0), DEG2RAD(0)};
for(int i=0;i<steps;i++)
{
oscillate(A,O, T, phase_diff);
}
}
OTTO机器人之胳膊测试
最新推荐文章于 2023-10-22 23:24:12 发布