OTTO机器人之胳膊测试

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

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
 

文章来源: recclay.blog.csdn.net，作者：ReCclay，版权归原作者所有，如需转载，请联系作者。

原文链接：recclay.blog.csdn.net/article/details/82721295