OTTO机器人之胳膊测试

版权声明：转载记得声明~~~ :) https://blog.csdn.net/ReCclay/article/details/82721295

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