OTTO机器人之太空舞步实现(带胳膊+不带胳膊)
机器人 实现 太空 不带
2023-09-11 14:20:36 时间
不带胳膊的版本
#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));
}
}
相关文章
- 机器人系统设计与制作：Python语言实现1.3 机器人上都有什么
- 机器人系统设计与制作：Python语言实现1.4 如何制作机器人
- 机器人系统设计与制作：Python语言实现2.3 安装LibreCAD、Blender和MeshLab
- 机器人系统设计与制作：Python语言实现2.4 用LibreCAD生成机器人的二维CAD图
- 机器人系统设计与制作：Python语言实现3.2 习题
- ROS机器人程序设计（原书第2版）1.4.4 安装ROS功能包
- ROS机器人程序设计（原书第2版）3.2.7 使用rqt_console和rqt_logger_level在运行时修改调试级别
- 《制造业中的机器人、自动化和系统集成》—— 2.4 机器人的好处
- 《自己动手做智能机器人》——1.4 成为卓越的机电一体化工程师要掌握哪些技能
- 《机器人构建实战》——1.2 机器人的定义
- 《机器人构建实战》——第2章 CDS5516数字舵机调试与参数设置
- ROS机器人操作系统查看机械臂当前位姿
- “杀手机器人”是否应该存在，我们又改如何应对
- 【转载】 【自监督学习机器人】谷歌大脑首次实现机器人端到端模仿人类动作 | 视频
- 练手的小项目(1)——智能聊天机器人
- Facebook聊天机器人可以订餐订桌了
- 使用图灵机器人实现虚拟客服