OTTO ROBOT PROJESİ

Birden fazla harekteleri gerçekleştiren otto robot ile dans hareketleri sağlayabiliyoruz. Bu şekilde birden fazla hareket gösteri ile güzel bir robot ortaya çıkmaktadır. Yükleme yaparken gerekli kütüphaneleri yüklemeyi unutmayın.

Proje Kodu

#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 run1(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);

#define ECHOPIN 9 // Pin to receive echo pulse

#define TRIGPIN 8 // Pin to send trigger pulse

int trimu;

int dansezpttine = 0;

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

if(EEPROM_TRIM){

for(int x=0;x<4;x++){

trimu=EEPROM.read(x);

if(trimu>128)trimu=trimu-256;

Serial.print("TRIM ");

Serial.print(x);

Serial.print(" en ");

Serial.println(trimu);

servo[x].SetTrim(trimu);

}

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

delay(500);

for(int i=0;i<4;i++) servo[i].detach();

pinMode(ECHOPIN, INPUT);

pinMode(TRIGPIN, OUTPUT);

}

// 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

// Start Ranging

digitalWrite(TRIGPIN, LOW);

delayMicroseconds(2);

digitalWrite(TRIGPIN, HIGH);

delayMicroseconds(10);

digitalWrite(TRIGPIN, LOW);

// Compute distance

float distance = pulseIn(ECHOPIN, HIGH);

distance= distance/58;

Serial.print(distance);

Serial.println("cm");

if ((distance >1) && (distance < 25))

{

Serial.println ("Danceeeee !");

dansezpttine = 1;

}

if (dansezpttine == 1)

{

servo[0].attach(PIN_RR);

servo[1].attach(PIN_RL);

servo[2].attach(PIN_YR);

servo[3].attach(PIN_YL);

if(EEPROM_TRIM){

for(int x=0;x<4;x++){

trimu=EEPROM.read(x);

if(trimu>128)trimu=trimu-256;

Serial.print("TRIM ");

Serial.print(x);

Serial.print(" en ");

Serial.println(trimu);

servo[x].SetTrim(trimu);

}

else{

servo[0].SetTrim(TRIM_RR);

servo[1].SetTrim(TRIM_RL);

servo[2].SetTrim(TRIM_YR);

servo[3].SetTrim(TRIM_YL);

}

delay(100);

dance();

delay(500);

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

delay(500);

for(int i=0;i<4;i++) servo[i].detach();

dansezpttine = 0;

}

delay(200);

//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);

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