OTTO机器人之太空舞步实现

其他 2018-09-21 13:10:47 阅读次数: 0
版权声明：转载记得声明~~~ :) https://blog.csdn.net/ReCclay/article/details/82710141

不带胳膊的版本

#include <Servo.h>
#include <Oscillator.h>
#include <EEPROM.h>

#define N_SERVOS 4
//-- First step: Configure the pins where the servos are attached
/*
         --------------- 
        |     O   O     |
        |---------------|
YR 3==> |               | <== YL 2
         --------------- 
            ||     ||
            ||     ||
RR 5==>   -----   ------  <== RL 4
         |-----   ------|
*/
#define EEPROM_TRIM false 
// Activate to take callibration data from internal memory
#define TRIM_RR 7
#define TRIM_RL 4
#define TRIM_YR  4
#define TRIM_YL -7
//OTTO.setTrims(-7,-4,-4,7);

#define PIN_RR 5
#define PIN_RL 4
#define PIN_YR 3
#define PIN_YL 2

#define INTERVALTIME 10.0 

Oscillator servo[N_SERVOS];

void goingUp(int tempo);
void drunk (int tempo);
void noGravity(int tempo);
void kickLeft(int tempo);
void kickRight(int tempo);
void run(int steps, int T=500);
void walk(int steps, int T=1000);
void backyard(int steps, int T=3000);
void backyardSlow(int steps, int T=5000);
void turnLeft(int steps, int T=3000);
void turnRight(int steps, int T=3000);
void moonWalkLeft(int steps, int T=1000);
void moonWalkRight(int steps, int T=1000);
void crusaito(int steps, int T=1000);
void swing(int steps, int T=1000);
void upDown(int steps, int T=1000);
void flapping(int steps, int T=1000);

void setup()
{
  Serial.begin(19200);

  servo[0].attach(PIN_RR);
  servo[1].attach(PIN_RL);
  servo[2].attach(PIN_YR);
  servo[3].attach(PIN_YL);

  //EEPROM.write(0,TRIM_RR);
  //EEPROM.write(1,TRIM_RL);
  //EEPROM.write(2,TRIM_YR);
  //EEPROM.write(3,TRIM_YL);

  int trim;

  if(EEPROM_TRIM){
    for(int x=0;x<4;x++){
      trim=EEPROM.read(x);
      if(trim>128)trim=trim-256;
      Serial.print("TRIM ");
      Serial.print(x);
      Serial.print(" en ");
      Serial.println(trim);
      servo[x].SetTrim(trim);
    }
  }
  else{
    servo[0].SetTrim(TRIM_RR);
    servo[1].SetTrim(TRIM_RL);
    servo[2].SetTrim(TRIM_YR);
    servo[3].SetTrim(TRIM_YL);
  }

  for(int i=0;i<4;i++) servo[i].SetPosition(90);
}

// TEMPO: 121 BPM
int t=495;
double pause=0;

void loop()
{
 // if(Serial.available()){
  //  char init = Serial.read();
   // if (init=='X'){
   //   delay(4000); //3000 - 4500

dance();


//for(int i=0;i<4;i++) servo[i].SetPosition(90);


          for(int i=0;i<4;i++) servo[i].SetPosition(90);
   // }
 // }
}

void dance(){
  primera_parte();
  segunda_parte();
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  primera_parte(); 
  crusaito(1,t*8);
  crusaito(1,t*7);

  for (int i=0; i<16; i++){
    flapping(1,t/4);
    delay(3*t/4);
  }

  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);
  moonWalkRight(4,t*2);
  moonWalkLeft(4,t*2);

  drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  drunk(t*4);
  kickLeft(t);
  kickRight(t);
  drunk(t*8);
  drunk(t*4);
  drunk(t/2);
  delay(t*4); 

  drunk(t/2);

  delay(t*4); 
  walk(2,t*2);
  backyard(2,t*2);
  goingUp(t*2);
  goingUp(t*1);
  noGravity(t*2);
  crusaito(1,t*2);
  crusaito(1,t*8);
  crusaito(1,t*2);
  crusaito(1,t*8);
  crusaito(1,t*2);
  crusaito(1,t*3);

  delay(t);
  primera_parte();
    for (int i=0; i<32; i++){
    flapping(1,t/2);
    delay(t/2);
  }

  for(int i=0;i<4;i++) servo[i].SetPosition(90);
}




////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////FUNCIONES DE CONTROL//////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]){
  for (int i=0; i<4; 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<4; i++){
        servo[i].refresh();
     }
  }
}

unsigned long final_time;
unsigned long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS]; 
int oldPosition[]={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];
  }   
}


//////////////////////////////////////////////////////////////////////////////
////////////////////////////////PASOS DE BAILE////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

void goingUp(int tempo){

      pause=millis();
      for(int i=0;i<4;i++) servo[i].SetPosition(90);
      delay(tempo);
      servo[0].SetPosition(80);
      servo[1].SetPosition(100);
      delay(tempo);
      servo[0].SetPosition(70);
      servo[1].SetPosition(110);
      delay(tempo);
      servo[0].SetPosition(60);
      servo[1].SetPosition(120);
      delay(tempo);
      servo[0].SetPosition(50);
      servo[1].SetPosition(130);
      delay(tempo);
      servo[0].SetPosition(40);
      servo[1].SetPosition(140);
      delay(tempo);
      servo[0].SetPosition(30);
      servo[1].SetPosition(150);
      delay(tempo);
      servo[0].SetPosition(20);
      servo[1].SetPosition(160);
      delay(tempo);

      while(millis()<pause+8*t);

}

void primera_parte(){

  int move1[4] = {60,120,90,90};
  int move2[4] = {90,90,90,90};
  int move3[4] = {40,140,90,90};

  for(int x=0; x<3; x++){
    for(int i=0; i<3; i++){
      lateral_fuerte(1,t/2);
      lateral_fuerte(0,t/4);
      lateral_fuerte(1,t/4);
      delay(t);
    }

    pause=millis();
    for(int i=0;i<4;i++) servo[i].SetPosition(90);
    moveNServos(t*0.4,move1);
    moveNServos(t*0.4,move2);
    while(millis()<(pause+t*2));
  }

  for(int i=0; i<2; i++){
    lateral_fuerte(1,t/2);
    lateral_fuerte(0,t/4);
    lateral_fuerte(1,t/4);
    delay(t);
  }

  pause=millis();
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  crusaito(1,t*1.4);
  moveNServos(t*1,move3);
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  while(millis()<(pause+t*4));
}

void segunda_parte(){

  int move1[4] = {90,90,80,100};
  int move2[4] = {90,90,100,80};
  int move3[4] = {90,90,80,100};
  int move4[4] = {90,90,100,80};

  int move5[4] = {40,140,80,100};
  int move6[4] = {40,140,100,80};
  int move7[4] = {90,90,80,100};
  int move8[4] = {90,90,100,80};

  int move9[4] = {40,140,80,100};
  int move10[4] = {40,140,100,80};
  int move11[4] = {90,90,80,100};
  int move12[4] = {90,90,100,80};

  for(int x=0; x<7; x++){ 
    for(int i=0; i<3; i++){
      pause=millis();
      moveNServos(t*0.15,move1);
      moveNServos(t*0.15,move2);
      moveNServos(t*0.15,move3);
      moveNServos(t*0.15,move4);
      while(millis()<(pause+t));
    }
    pause=millis();
    moveNServos(t*0.15,move5);
    moveNServos(t*0.15,move6);
    moveNServos(t*0.15,move7);
    moveNServos(t*0.15,move8);
    while(millis()<(pause+t));
  }

  for(int i=0; i<3; i++){
    pause=millis();
    moveNServos(t*0.15,move9);
    moveNServos(t*0.15,move10);
    moveNServos(t*0.15,move11);
    moveNServos(t*0.15,move12);
    while(millis()<(pause+t));
  }
}

void lateral_fuerte(boolean side, int tempo){

  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  if (side) servo[0].SetPosition(40);
  else servo[1].SetPosition(140);
  delay(tempo/2);
  servo[0].SetPosition(90);
  servo[1].SetPosition(90);
  delay(tempo/2);

}

void drunk (int tempo){

  pause=millis();

  int move1[] = {60,70,90,90};
  int move2[] = {110,120,90,90};
  int move3[] = {60,70,90,90};
  int move4[] = {110,120,90,90};

  moveNServos(tempo*0.235,move1);
  moveNServos(tempo*0.235,move2);
  moveNServos(tempo*0.235,move3);
  moveNServos(tempo*0.235,move4);
  while(millis()<(pause+tempo));

}


void noGravity(int tempo){

  int move1[4] = {120,140,90,90};
  int move2[4] = {140,140,90,90};
  int move3[4] = {120,140,90,90};
  int move4[4] = {90,90,90,90};


  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  for(int i=0;i<N_SERVOS;i++) oldPosition[i]=90;
  moveNServos(tempo*2,move1);
  moveNServos(tempo*2,move2);
  delay(tempo*2);
  moveNServos(tempo*2,move3);
  moveNServos(tempo*2,move4);



}

void kickLeft(int tempo){
  for(int i=0;i<4;i++) servo[i].SetPosition(90);
  delay(tempo);
  servo[0].SetPosition(50); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(30); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(30); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(80); //pie derecho
  servo[1].SetPosition(70); //pie izquiero
  delay(tempo);
}

void kickRight(int tempo){
for(int i=0;i<4;i++) servo[i].SetPosition(90);
  delay(tempo);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(130); //pie izquiero
  delay(tempo);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(100); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(150); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(80); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(150); //pie izquiero
  delay(tempo/4);
  servo[0].SetPosition(110); //pie derecho
  servo[1].SetPosition(100); //pie izquiero
  delay(tempo);
}

void walk(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void run(int steps, int T){
    int A[4]= {10, 10, 10, 10};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void backyard(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void backyardSlow(int steps, int T){
    int A[4]= {15, 15, 30, 30};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}


void turnLeft(int steps, int T){
    int A[4]= {20, 20, 10, 30};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void turnRight(int steps, int T){
    int A[4]= {20, 20, 30, 10};
    int O[4] = {0, 0, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void moonWalkRight(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15 ,15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void moonWalkLeft(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15, 15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void crusaito(int steps, int T){
    int A[4]= {25, 25, 30, 30};
    int O[4] = {- 15, 15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), 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[4]= {25, 25, 0, 0};
    int O[4] = {-15, 15, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void upDown(int steps, int T){
    int A[4]= {25, 25, 0, 0};
    int O[4] = {-15, 15, 0, 0};
    double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270), DEG2RAD(270)};

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void flapping(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0], A[1], 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(-90)};

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

void test(int steps, int T){
    int A[4]= {15, 15, 8, 8};
    int O[4] = {-A[0] + 10, A[1] - 10, 0, 0};
    double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(-90)};

    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);
}

带胳膊的版本

#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];

int t=495;
double pause=0;

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() 
{ 
    dance();
}

void dance()
{
    primera_parte();
    segunda_parte();
    moonWalkLeft(4,t*2);
    moonWalkRight(4,t*2);
    moonWalkLeft(4,t*2);
    moonWalkRight(4,t*2);
    primera_parte(); 
    crusaito(1,t*8);
    crusaito(1,t*7);

    for (int i=0; i<16; i++)
    {
        flapping(1,t/4);
        delay(3*t/4);
    }

    moonWalkRight(4,t*2);
    moonWalkLeft(4,t*2);
    moonWalkRight(4,t*2);
    moonWalkLeft(4,t*2);

    drunk(t*4);
    drunk(t*4);
    drunk(t*4);
    drunk(t*4);
    kickLeft(t);
//  kickRight(t);
    drunk(t*8);
    drunk(t*4);
    drunk(t/2);
    delay(t*4); 

    drunk(t/2);

    delay(t*4); 
    walk(2,t*2);
//  backyard(2,t*2);
//  goingUp(t*2);
//  goingUp(t*1);
//  noGravity(t*2);
    crusaito(1,t*2);
    crusaito(1,t*8);
    crusaito(1,t*2);
    crusaito(1,t*8);
    crusaito(1,t*2);
    crusaito(1,t*3);

    delay(t);
    primera_parte();
    for (int i=0; i<32; i++)
    {
        flapping(1,t/2);
        delay(t/2);
    }

    for(int i=0;i<4;i++) 
    {
        servo[i].SetPosition(90);
    }
}



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 run(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);
    }
}

void walk(int steps, int T)
{
    int A[6]= {10, 10, 10, 10, 20, 20}; // 移动幅度 (引脚对应分别是：右脚 左脚 右腿 左腿 右胳膊 左胳膊)
    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);
    }
}

void crusaito(int steps, int T)
{
    int A[6]= {25, 25, 30, 30, 30, 30};
    int O[6] = {- 15, 15, 0, 0, 0, 0};
    double phase_diff[6] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90), DEG2RAD(0), DEG2RAD(0)}; 

    for(int i=0;i<steps;i++)
    {
        oscillate(A,O, T, phase_diff);
    }
}

void moonWalkLeft(int steps, int T)
{
    //向左移动，左胳膊在上大幅度摆，右胳膊在下小幅度摆

    int A[6]= {25, 25, 0, 0, 10, 30};
    int O[6] = {-15, 15, 0, 0, 40, 0};
    double phase_diff[6] = {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90), DEG2RAD(0), DEG2RAD(0)}; 

    for(int i=0;i<steps;i++)
    {
        oscillate(A,O, T, phase_diff);
    }
}

void moonWalkRight(int steps, int T)
{
    //向右移动，右胳膊在上大幅度摆，左胳膊在下小幅度摆

    int A[6]= {25, 25, 0, 0, 30, 10};
    int O[6] = {-15 ,15, 0, 0, 0, -40};
    double phase_diff[6] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)}; 

    for(int i=0;i<steps;i++)
    {
        oscillate(A,O, T, phase_diff);
    }
}

void flapping(int steps, int T)
{
    int A[6]= {15, 15, 8, 8, 30, 30};
    int O[6] = {-A[0], A[1], 0, 0, 0, 30};
    double phase_diff[6] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(-90), DEG2RAD(0), DEG2RAD(180)};

    for(int i=0;i<steps;i++)
    {
        oscillate(A,O, T, phase_diff);
    }
}

void drunk (int tempo)
{
    pause=millis();

    int move1[6] = {60,70,90,90,0,180};
    int move2[6] = {110,120,90,90,45,145};
    int move3[6] = {60,70,90,90,0,180};
    int move4[6] = {110,120,90,90,45,145};

    moveNServos(tempo*0.235, move1);
    moveNServos(tempo*0.235, move2);
    moveNServos(tempo*0.235, move3);
    moveNServos(tempo*0.235, move4);
    while(millis()<(pause+tempo));
}

void kickLeft(int tempo)
{
    for(int i=0;i<4;i++) 
    {
      servo[i].SetPosition(90);
    }
    delay(tempo);
    servo[0].SetPosition(50); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo);
    servo[0].SetPosition(80); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo/4);
    servo[0].SetPosition(30); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo/4);
    servo[0].SetPosition(80); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo/4);
    servo[0].SetPosition(30); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo/4);
    servo[0].SetPosition(80); //pie derecho
    servo[1].SetPosition(70); //pie izquiero
    delay(tempo);
}
//////////////////////////////////////////////////////
/////////////////////基 本 动 作(以上) ///////////////
//////////////////////////////////////////////////////


//单腿原地滑步+胳膊扭动，其余部位保持90°。 
//tempo表示对应的时间间隔,side-1右滑，side-0左滑
void lateral_fuerte(boolean side, int tempo) 
{
    for(int i=0;i<6;i++)//舵机归位 
    {
        servo[i].SetPosition(90);
    }
    if (side)
    {   
        servo[0].SetPosition(40);
        servo[4].SetPosition(140);
        delay(tempo/10);
    }
    else 
    {
        servo[1].SetPosition(140);
        servo[5].SetPosition(40);
        delay(tempo/10);
    }
    delay(tempo/2);
    servo[0].SetPosition(90);
    servo[1].SetPosition(90);
    servo[4].SetPosition(90);
    servo[5].SetPosition(90);
    delay(tempo/2);
}

unsigned long final_time;
unsigned long interval_time;
int oneTime;
int iteration;
float increment[N_SERVOS]; 
int oldPosition[]={90,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)
    { 
        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];
    }   
}


//第一套动作 - 滑步
void primera_parte()
{
    int move1[6] = {60,120,90,90,140,40};
    int move2[6] = {90,90,90,90,90,90};
    int move3[6] = {40,140,90,90,40,140};

    for(int x=0; x<3; x++)
    {
        for(int i=0; i<3; i++)
        {
            lateral_fuerte(1,t/2);
            lateral_fuerte(0,t/4);
            lateral_fuerte(1,t/4);
            delay(t);
        }

        pause=millis();
        for(int i=0;i<6;i++) // 舵机归位 
        {
            servo[i].SetPosition(90);
        }
        moveNServos(t*0.4,move1);
        moveNServos(t*0.4,move2);
        while(millis()<(pause+t*2));
    }

    for(int i=0; i<2; i++)
    {
        lateral_fuerte(1,t/2);
        lateral_fuerte(0,t/4);
        lateral_fuerte(1,t/4);
        delay(t);
    }

    pause=millis();
    for(int i=0;i<6;i++) // 舵机归位  
    {
        servo[i].SetPosition(90);
    }
    crusaito(1,t*1.4);
    moveNServos(t*1,move3);
    for(int i=0;i<6;i++) // 舵机归位  
    {
        servo[i].SetPosition(90);
    }
    while(millis()<(pause+t*4));
}


//第二套动作 - 抖腿
void segunda_parte()
{
    //抖腿
    int move1[6] = {90,90,80,100,0,0};
    int move2[6] = {90,90,100,80,0,0};
    int move3[6] = {90,90,80,100,0,0};
    int move4[6] = {90,90,100,80,0,0};

    //小高潮
    int move5[6] = {40,140,80,100,90,90};
    int move6[6] = {40,140,100,80,90,90};
    int move7[6] = {90,90,80,100,90,90};
    int move8[6] = {90,90,100,80,90,90};

    //大高潮
    int move9[6] = {40,140,80,100,0,180};
    int move10[6] = {40,140,100,80,40,140};
    int move11[6] = {90,90,80,100,0,180};
    int move12[6] = {90,90,100,80,40,140};

    for(int x=0; x<7; x++)
    { 
        for(int i=0; i<3; i++)
        {
            pause=millis();
            moveNServos(t*0.15,move1);
            moveNServos(t*0.15,move2);
            moveNServos(t*0.15,move3);
            moveNServos(t*0.15,move4);
            while(millis()<(pause+t));
        }
        pause=millis();
        moveNServos(t*0.15,move5);
        moveNServos(t*0.15,move6);
        moveNServos(t*0.15,move7);
        moveNServos(t*0.15,move8);
        while(millis()<(pause+t));
    }

    for(int i=0; i<3; i++)
    {
        pause=millis();
        moveNServos(t*0.15,move9);
        moveNServos(t*0.15,move10);
        moveNServos(t*0.15,move11);
        moveNServos(t*0.15,move12);
        while(millis()<(pause+t));
    }
}

猜你喜欢

转载自blog.csdn.net/ReCclay/article/details/82710141
今日推荐
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
报告：Django 仍然是 74% 开发者的首选
《2024 年一季度互联网投融资运行情况》研究报告
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
周排行
BPM为企业带来的实际利益
好程序员web前端分享css常用属性缩写
Java文件下载（excel）
css样式的动态添加及显示和隐藏等零碎用法
axios全局配置以及拦截器
使用Logstash来实时同步MySQL和log日志数据到ES
C++获取当前时间（年月日、时分秒、毫秒）
Odoo产品分析 (四) -- 工具板块(11) -- 网站即时聊天(1)
Java环境配置正确，但是java、javac、java -version均返回“不是内部或外部命令，也不是可运行的程序或批处理文件”？
01 官网下载各种CentOS教程（超详细版）
每日归档
更多
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022