ROBOT ARAÑA
Este kit de robótica para hacer uno mismo permite a cualquiera poder construir su propio robot de cuatro patas y es totalmente compatible con una placa Arduino.
Kit didáctico Araña de 4 patas y 8 grados de libertad.
Puede funcionar de dos maneras, o bien a través de su mando o bien a través de programación de movimientos.
Esta basado en el popular Meped. Es compatible con Arduino o cualquier tarjeta de desarrollo o microcontrolador con salidas digitales o de PWM.
Es un proyecto tecnológico para hacer que hasta los más pequeños aprendan a construir su propio brazo robótico. Es económico, divertido de armar y una gran herramienta educativa.
El Kit Incluye:
- 3 laminas de MDF precortadas a láser
- 8 miniservos
- Tornilleria.
- Manual de ensamblado.
- Plataforma de control arduino nano.
- Shield de expansión para arduino nano
- Código en arduino.
-Totalmente cortado a láser.
-Diseñado para optimizar la fuerza de los miniservos.
-Listo para que instales tus miniservos.
-Píntalo del color que mas te guste!
-Se arma fácilmente.
PASOS DE ARMADO
CÓDIGO ROBOT ARAÑA 1
Si montó y calibró
correctamente el robot, el robot dará cuatro ciclos de pasos hacia adelante y
cuatro ciclos de pasos hacia atrás. En caso de dificultades y
pérdida de equilibrio, podemos recomendar lo siguiente:
- ·
Juega
con las variables da, db, dc, dd.
- · Tenga en cuenta que para los pasos adelante y atrás son diferentes;
- ·
Asegúrese
de que todas las piezas estén bien apretadas y que no haya retroceso;
- ·
Equilibre
el equipo colgante del robot de tal manera que el centro de gravedad caiga
exactamente en el centro geométrico de la plataforma del robot
CÓDIGO 1
#include <Servo.h>
// crear el objeto servo
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;
Servo myservo7;
Servo myservo8;
void setup()
{
// asigna servo al pin
myservo1.attach(2); //
myservo2.attach(3); //
myservo3.attach(4); //
myservo4.attach(5); //
myservo5.attach(6); //
myservo6.attach(7); //
myservo7.attach(8); //
myservo8.attach(10); //
}
int da=0, db=0, dc=0, dd=0;
int a90=(90+da), a120=(120+da), a150=(150+da), a180=(180+da);
int b0=(0+db), b30=(30+db), b60=(60+db), b90=(90+db);
int c90=(90+dc), c120=(120+dc), c150=(150+dc), c180=(180+dc);
int d0=(0+dd), d30=(30+dd), d60=(60+dd), d90=(90+dd);
// fichas de ajuste en el rango de trabajo del medio.
int s11 = 90;
int s12 = 90;
int s21 = 90;
int s22 = 90;
int s31 = 18;
int s32 = 18;
int s41 = 18;
int s42 = 18;
void loop()
{
forvard();forvard();forvard();forvard();delay(2000);
back ();back ();back ();back ();delay(2000);
}
void forvard()
{
da=0, db=0+36, dc=-36, dd=0;
a90=(90+da), a120=(120+da), a150=(150+da), a180=(180+da);
b0=(0+db), b30=(30+db), b60=(60+db), b90=(90+db);
c90=(90+dc), c120=(120+dc), c150=(150+dc), c180=(180+dc);
d0=(0+dd), d30=(30+dd), d60=(60+dd), d90=(90+dd);
srv(a180, b0 ,c120, d60, 42,42,42,42, 1,3,1,1); // шаг 1
srv( a90, b30, c90, d30, 6,42,42,42, 3,1,1,1); // шаг 2
srv( a90, b30, c90, d30, 42,42,42,42, 3,1,1,1); // шаг 2
srv(a120, b60, c180, d0, 42,42,6,42, 1,1,3,1); // шаг 3
srv(a120, b60, c180, d0, 42,42,42,42, 1,1,3,1); // шаг 3
srv(a150, b90, c150, d90, 42,42,42,6, 1,1,1,3); // шаг 4
srv(a150, b90, c150, d90, 42,42,42,42, 1,1,1,3); // шаг 4
srv(a180, b0, c120, d60, 42,6,42,42, 1,3,1,1); // шаг 1
}
void back ()
{
da=-36, db=0, dc=0, dd=36;
a90=(90+da), a120=(120+da), a150=(150+da), a180=(180+da);
b0=(0+db), b30=(30+db), b60=(60+db), b90=(90+db);
c90=(90+dc), c120=(120+dc), c150=(150+dc), c180=(180+dc);
d0=(0+dd), d30=(30+dd), d60=(60+dd), d90=(90+dd);
srv(a180, b0, c120, d60, 42,42,42,42, 3,1,1,1); // шаг 1
srv(a150, b90, c150, d90, 42, 6,42,42, 1,3,1,1); // шаг 4
srv(a150, b90, c150, d90, 42,42,42,42, 1,3,1,1); // шаг 4
srv(a120, b60, c180, d0, 42,42,42,6, 1,1,1,3); // шаг 3
srv(a120, b60, c180, d0, 42,42,42,42, 1,1,1,3); // шаг 3
srv( a90, b30, c90, d30, 42,42, 6,42, 1,1,3,1); // шаг 2
srv( a90, b30, c90, d30, 42,42,42,42, 1,1,3,1); // шаг 2
srv(a180, b0, c120, d60, 6,42,42,42, 3,1,1,1); // шаг 1
}
void srv( int p11, int p21, int p31, int p41, int p12, int p22, int p32, int p42, int sp1, int sp2, int sp3, int sp4)
{
while((s11!=p11) || (s21!=p21) || (s31!=p31) || (s41!=p41) || (s12!=p12) || (s22!=p22) || (s32!=p32) ||
(s42!=p42))
{
if (s11<p11) s11=s11+sp1;
if (s11>p11) s11=s11-sp1;
if (s21<p21) s21=s21+sp2;
if (s21>p21) s21=s21-sp2;
if (s31<p31) s31=s31+sp3;
if (s31>p31) s31=s31-sp3;
if (s41<p41) s41=s41+sp4;
if (s41>p41) s41=s41-sp4;
if (s12<p12) s12=s12+sp1;
if (s12>p12) s12=s12-sp1;
if (s22<p22) s22=s22+sp2;
if (s22>p22) s22=s22-sp2;
if (s32<p32) s32=s32+sp3;
if (s32>p32) s32=s32-sp3;
if (s42<p42) s42=s42+sp4;
if (s42>p42) s42=s42-sp4;
myservo1.write(s11);
myservo3.write(s21);
myservo5.write(s31);
myservo7.write(s41);
myservo2.write(s12);
myservo4.write(s22);
myservo6.write(s32);
myservo8.write(s42);
delay(20);
}
}
CÓDIGO 2 ------------------------------
/***************************************************/
/*********************************************************************************************************************/
#include <Servo.h>
// set the names of the servos
/*
hip1 = parte delantera derecha de la cadera
knee11 = rodilla derecha delantera
hip2 = parte delantera izquierda de la cadera
knee21 = rodilla delantera izquierda
hip3 = parte posterior izquierda de la cadera
knee31 = rodilla trasera izquierda
hip4 = cadera trasera derecha
knee41 = rodilla derecha trasera
*/
Servo hip1;
Servo knee11;
Servo hip2;
Servo knee21;
Servo hip3;
Servo knee31;
Servo hip4;
Servo knee41;
// Establecer variables necesarias para algunas tareas
int b;
int x;
int up;
int up2;
int up3;
int down;
int down2;
int steps;
int rightsteps;
int leftsteps;
//////////////////////////////////////
void setup()
{
pinMode(13, OUTPUT); // LED pin
// Asignar servos a pines y servos centrales.
hip1.attach(5);
hip1.write(90);
knee11.attach(6);
knee11.write(90);
hip2.attach(7);
hip2.write(90);
knee21.attach(8);
knee21.write(90);
hip3.attach(9);
hip3.write(90);
knee31.attach(10);
knee31.write(90);
hip4.attach(11);
hip4.write(90);
knee41.attach(12);
knee41.write(90);
}
//////////////////////////////////////
void idle() // este es el retraso entre los pasos -> velocidad de caminar
{
delay(70); // si se configura en un número más grande (más demora entre los pasos -> caminar más lento)
//verá el patrón de caminar más claramente
}
//////////////////////////////////////
void test() /*solo para la depuración -> si necesita un retraso entre las subrutinas,
puede dejar que el LED parpadee como un indicador de que algo está funcionando. */
{
for(b = 0; b < 2; b++) // esto deja que el LED parpadee 5 veces
{
digitalWrite(13, HIGH); // enciende el LED
delay(300); // espera .5 segundos
digitalWrite(13, LOW); // apagar el LED
delay(300);
}
}
//////////////////////////////////////
void standup()
{
up2 = 90;
up3 = 90;
hip1.write(70);
hip2.write(80);
hip3.write(120);
hip4.write(70);
for(up = 90; up < 170; up++)
{
knee11.write(up);
up2 = up2 - 1;
knee21.write(up2);
delay(20);
}
for(up = 90; up < 180; up++)
{
knee31.write(up);
up3 = up3 - 1;
knee41.write(up3);
delay(15);
}
}
//////////////////////////////////////
void sleep()
{
// hips
hip1.write(70);
hip2.write(90);
hip3.write(70);
hip4.write(90);
// knees
knee11.write(0);
knee21.write(180);
knee31.write(0);
knee41.write(180);
}
//////////////////////////////////////
void stand()
{
hip1.write(70);
knee11.write(170);
delay(20);
hip2.write(80);
knee21.write(20);
delay(20);
hip3.write(120);
knee31.write(170);
delay(20);
hip4.write(70);
knee41.write(20);
delay(20);
}
//////////////////////////////////////
void forward()
{
// levante la rodilla delantera derecha, mueva la cadera delantera derecha hacia adelante y
//la cadera derecha hacia atrás, la rodilla delantera derecha inferior
knee11.write(150);
idle();
hip1.write(45);
hip4.write(90);
idle();
knee11.write(170);
// levante la rodilla trasera izquierda, mueva la parte trasera de la cadera izquierda hacia adelante y
//la parte delantera derecha de la cadera hacia atrás, la parte inferior trasera de la rodilla izquierda
knee31.write(150);
idle();
hip3.write(120);
hip1.write(110);
idle();
knee31.write(170);
// levante la rodilla delantera izquierda, mueva la cadera izquierda delantera hacia adelante y
//la cadera izquierda trasera hacia atrás, la rodilla izquierda delantera delantera inferior
knee21.write(40);
idle();
hip2.write(110);
hip3.write(60);
idle();
knee21.write(20);
// levante la rodilla derecha posterior, mueva la cadera posterior derecha hacia adelante y
//la cadera izquierda hacia atrás, la rodilla derecha posterior inferior
knee41.write(40);
idle();
hip4.write(30);
hip2.write(80);
idle();
knee41.write(20);
idle();
}
/////////////////////////////////////
void rightturn()
{
// levante la rodilla delantera derecha, mueva la cadera delantera derecha hacia adelante y
//la cadera derecha hacia atrás, la rodilla delantera derecha inferior
knee11.write(150);
idle();
hip1.write(30);
hip4.write(90);
idle();
knee11.write(170);
// levante la rodilla trasera izquierda, mueva la parte trasera de la cadera izquierda hacia adelante y
//la parte delantera derecha de la cadera hacia atrás, la parte inferior trasera de la rodilla izquierda
knee31.write(150);
idle();
hip3.write(130);
hip1.write(100);
idle();
knee31.write(170);
// levante la rodilla delantera izquierda, mueva la cadera izquierda delantera hacia adelante y
//la cadera izquierda trasera hacia atrás, la rodilla izquierda delantera delantera inferior
knee21.write(40);
idle();
hip2.write(130);
hip3.write(50);
idle();
knee21.write(20);
// levante la rodilla derecha posterior, mueva la cadera posterior derecha hacia adelante y
//la cadera izquierda hacia atrás, la rodilla derecha posterior inferior
knee41.write(40);
idle();
hip4.write(90);
hip2.write(50);
idle();
knee41.write(20);
idle();
}
/////////////////////////////////////
void leftturn()
{
// levante la rodilla delantera derecha, mueva la cadera delantera derecha hacia adelante y
//la cadera derecha hacia atrás, la rodilla delantera derecha inferior
knee11.write(150);
idle();
hip1.write(30);
hip4.write(100);
idle();
knee11.write(170);
// levante la rodilla trasera izquierda, mueva la parte trasera de la cadera izquierda hacia adelante y
//la parte delantera derecha de la cadera hacia atrás, la parte inferior trasera de la rodilla izquierda
knee31.write(150);
idle();
hip3.write(90);
hip1.write(120);
idle();
knee31.write(170);
// levante la rodilla delantera izquierda, mueva la cadera izquierda delantera hacia adelante y
//la cadera izquierda trasera hacia atrás, la rodilla izquierda delantera delantera inferior
knee21.write(40);
idle();
hip2.write(90);
hip3.write(60);
idle();
knee21.write(20);
// levante la rodilla derecha posterior, mueva la cadera posterior derecha hacia adelante y
//la cadera izquierda hacia atrás, la rodilla derecha posterior inferior
knee41.write(40);
idle();
hip4.write(30);
hip2.write(80);
idle();
knee41.write(20);
idle();
}
/////////////////////////////////////
void laydowndown() // ACUESTATE
{
hip1.write(70);
hip2.write(80);
for (down = 170; down > 90; down = down - 1){
knee11.write(down);
down2 = 190 - down;
knee21.write(down2);
delay(15);
}
delay(1000);
hip3.write(80);
hip4.write(70);
for (down = 170; down > 90; down = down - 1){
knee31.write(down);
down2 = 190 - down;
knee41.write(down2);
delay(15);
}
}
/////////////////////////////////////
void gym() // ok, esto no es muy serio, pero necesitaba animarme un poco ;-) solo veo ...
{
int y;
hip1.write(70);
knee31.write(130);
delay(200);
knee21.write(40);
hip2.write(110);
knee21.write(20);
delay(20);
knee11.write(60);
delay(20);
hip3.write(120);
delay(20);
hip4.write(70);
knee41.write(20);
delay(20);
}
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
void loop()
{
sleep(); // empezar desde esta posición
test(); // Este es el retraso entre cada patrón.
standup(); // Deje que el robot pase del estado de reposo al puesto de pie.
test(); // Este es el retraso entre cada patrón.
while(steps < 10){ // solo un bucle de 10 pasos hacia adelante ya que todavía no tiene ojos
forward();
steps++;
}
test();
while(rightsteps < 10){ // solo un bucle de 10 pasos a la derecha ya que todavía no tiene ojos
rightturn();
rightsteps++;
}
test(); // Este es el retraso entre cada patrón.
while(leftsteps < 10){ // solo un bucle de 10 pasos a la izquierda ya que todavía no tiene ojos
leftturn();
leftsteps++;
}
gym(); // haciendo algún movimiento divertido (creo)
test(); // Este es el retraso entre cada patrón.
laydowndown(); // acostarse después de estos ejercicios
test(); // Este es el retraso entre cada patrón.
}
----------------------------
CODIGO 3
//==========================================================================================
//
// Program for controlling a mePed Robot using an IR Remote
//
// The mePed is an open source quadruped robot designed by Scott Pierce of
// Spierce Technologies (www.meped.io & www.spiercetech.com)
//
// This program is based on code written by Alexey Butov (www.alexeybutov.wix.com/roboset)
//
//==========================================================================================
#include <IRremote.h> // include IR Remote library
#include <Servo.h> // include servo library
//===== Globals ============================================================================
// Define USRF pins and variables
#define trigPin A3
#define echoPin A2
#define INCH 0
#define CM 1
// Define IR Remote Button Codes
#define irUp 16736925
#define irDown 16754775
#define irRight 16761405
#define irLeft 16720605
#define irOK 16712445
#define ir1 16738455
#define ir2 16750695
#define ir3 16756815
#define ir4 16724175
#define ir5 16718055
#define ir6 16743045
#define ir7 16716015
#define ir8 16726215
#define ir9 16734885
#define ir0 16730805
#define irStar 16728765
#define irPound 16732845
#define irRepeat 4294967295
// calibration
int da = -12, // Left Front Pivot
db = 10, // Left Back Pivot
dc = -18, // Right Back Pivot
dd = 12; // Right Front Pivot
// servo initial positions + calibration
int a90 = (90 + da),
a120 = (120 + da),
a150 = (150 + da),
a180 = (180 + da);
int b0 = (0 + db),
b30 = (30 + db),
b60 = (60 + db),
b90 = (90 + db);
int c90 = (90 + dc),
c120 = (120 + dc),
c150 = (150 + dc),
c180 = (180 + dc);
int d0 = (0 + dd),
d30 = (30 + dd),
d60 = (60 + dd),
d90 = (90 + dd);
// start points for servo
int s11 = 90; // Front Left Pivot Servo
int s12 = 90; // Front Left Lift Servo
int s21 = 90; // Back Left Pivot Servo
int s22 = 90; // Back Left Lift Servo
int s31 = 90; // Back Right Pivot Servo
int s32 = 90; // Back Right Lift Servo
int s41 = 90; // Front Right Pivot Servo
int s42 = 90; // Front Right Lift Servo
int f = 0;
int b = 0;
int l = 0;
int r = 0;
int spd = 3; // Speed of walking motion, larger the number, the slower the speed
int high = 0; // How high the robot is standing
// Define 8 Servos
Servo myServo1; // Front Left Pivot Servo
Servo myServo2; // Front Left Lift Servo
Servo myServo3; // Back Left Pivot Servo
Servo myServo4; // Back Left Lift Servo
Servo myServo5; // Back Right Pivot Servo
Servo myServo6; // Back Right Lift Servo
Servo myServo7; // Front Right Pivot Servo
Servo myServo8; // Front Right Lift Servo
// Set up IR Sensor
int irReceiver = 12; // Use pin D12 for IR Sensor
IRrecv irrecv(irReceiver); // create a new instance of the IR Receiver
decode_results results;
//==========================================================================================
//===== Setup ==============================================================================
void setup()
{
// Attach servos to Arduino Pins
myServo1.attach(2);
myServo2.attach(3);
myServo3.attach(4);
myServo4.attach(5);
myServo5.attach(6);
myServo6.attach(7);
myServo7.attach(8);
myServo8.attach(9);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
irrecv.enableIRIn(); //start the receiver
Serial.begin (9600);
}//setup
//==========================================================================================
//== Loop ==================================================================================
void loop()
{
unsigned long value;
unsigned long lastValue;
center_servos(); // Center all servos
high = 15; // Set hight to 15
spd = 3; // Set speed to 3
while (1 == 1) // Loop forever
{
if (irrecv.decode(&results)) // If we have received an IR signal
{
value = results.value;
if (value == irRepeat)
value = lastValue;
switch (value)
{
case irUp:
lastValue = irUp;
forward();
break;
case irDown:
lastValue = irDown;
back();
break;
case irRight:
lastValue = irRight;
turn_right();
break;
case irLeft:
lastValue = irLeft;
turn_left();
break;
case irOK:
lastValue = irOK;
break;
case ir1:
lastValue = ir1;
bow();
break;
case ir2:
lastValue = ir2;
wave();
break;
case ir3:
lastValue = ir3;
increase_speed();
break;
case ir4:
lastValue = ir4;
break;
case ir5:
lastValue = ir5;
break;
case ir6:
lastValue = ir6;
decrease_speed();
break;
case ir7:
lastValue = ir7;
break;
case ir8:
lastValue = ir8;
dance();
break;
case ir9:
lastValue = ir9;
break;
case ir0:
lastValue = ir0;
center_servos();
break;
case irStar:
lastValue = irStar;
trim_left();
break;
case irPound:
lastValue = irPound;
trim_right();
break;
default:
break;
}
irrecv.resume(); //next value
delay(50); // Pause for 50ms before executing next movement
}// if irrecv.decode
}//while
}//loop
void dance()
{
center_servos();
delay(100);
lean_left();
delay(300);
lean_right();
delay(300);
lean_left();
delay(300);
lean_right();
delay(300);
lean_left();
delay(300);
lean_right();
delay(300);
lean_left();
delay(300);
lean_right();
delay(800);
center_servos();
delay(300);
bow();
center_servos();
}
//== Wave ==================================================================================
void wave()
{
/*
myServo1 - Front Left Pivot Servo
myServo2 - Front Left Lift Servo
myServo3 - Back Left Pivot Servo
myServo4 - Back Left Lift Servo
myServo5 - Back Right Pivot Servo
myServo6 - Back Right Lift Servo
myServo7 - Front Right Pivot Servo
myServo8 - Front Right Lift Servo
*/
center_servos();
myServo4.write(45);
myServo6.write(45);
delay(200);
myServo8.write(0);
delay(200);
myServo7.write(180);
delay(200);
myServo7.write(30);
delay(300);
myServo7.write(180);
delay(300);
myServo7.write(30);
delay(300);
myServo7.write(s41);
delay(300);
myServo8.write(s42);
center_servos();
}
//== Bow ===================================================================================
void bow()
{
center_servos();
delay(200);
myServo2.write(15);
myServo8.write(15);
delay(700);
myServo2.write(90);
myServo8.write(90);
delay(700);
}
//== Lean_Left =============================================================================
void lean_left()
{
myServo2.write(15);
myServo4.write(15);
myServo6.write(150);
myServo8.write(150);
}
//== Lean_Right ======================================================
void lean_right()
{
myServo2.write(150);
myServo4.write(150);
myServo6.write(15);
myServo8.write(15);
}
//== Lean_Left =============================================================================
void trim_left()
{
da--; // Left Front Pivot
db--; // Left Back Pivot
dc--; // Right Back Pivot
dd--; // Right Front Pivot
}
//== Lean_Right ============================================================================
void trim_right()
{
da++; // Left Front Pivot
db++; // Left Back Pivot
dc++; // Right Back Pivot
dd++; // Right Front Pivot
}
//== Forward ======================================================
void forward()
{
// calculation of points
// Left Front Pivot
a90 = (90 + da),
a120 = (120 + da),
a150 = (150 + da),
a180 = (180 + da);
// Left Back Pivot
b0 = (0 + db),
b30 = (30 + db),
b60 = (60 + db),
b90 = (90 + db);
// Right Back Pivot
c90 = (90 + dc),
c120 = (120 + dc),
c150 = (150 + dc),
c180 = (180 + dc);
// Right Front Pivot
d0 = (0 + dd),
d30 = (30 + dd),
d60 = (60 + dd),
d90 = (90 + dd);
// set servo positions and speeds needed to walk forward one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(a180, b0 , c120, d60, 42, 33, 33, 42, 1, 3, 1, 1);
srv( a90, b30, c90, d30, 6, 33, 33, 42, 3, 1, 1, 1);
srv( a90, b30, c90, d30, 42, 33, 33, 42, 3, 1, 1, 1);
srv(a120, b60, c180, d0, 42, 33, 6, 42, 1, 1, 3, 1);
srv(a120, b60, c180, d0, 42, 33, 33, 42, 1, 1, 3, 1);
srv(a150, b90, c150, d90, 42, 33, 33, 6, 1, 1, 1, 3);
srv(a150, b90, c150, d90, 42, 33, 33, 42, 1, 1, 1, 3);
srv(a180, b0, c120, d60, 42, 6, 33, 42, 1, 3, 1, 1);
//srv(a180, b0, c120, d60, 42, 15, 33, 42, 1, 3, 1, 1);
}
//== Back ======================================================
void back ()
{
// set servo positions and speeds needed to walk backward one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(180, 0, 120, 60, 42, 33, 33, 42, 3, 1, 1, 1);
srv(150, 90, 150, 90, 42, 18, 33, 42, 1, 3, 1, 1);
srv(150, 90, 150, 90, 42, 33, 33, 42, 1, 3, 1, 1);
srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3);
srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3);
srv(90, 30, 90, 30, 42, 33, 18, 42, 1, 1, 3, 1);
srv(90, 30, 90, 30, 42, 33, 33, 42, 1, 1, 3, 1);
srv(180, 0, 120, 60, 6, 33, 33, 42, 3, 1, 1, 1);
}
//== Left ======================================================
void turn_left ()
{
// set servo positions and speeds needed to turn left one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv(150, 90, 90, 30, 42, 6, 33, 42, 1, 3, 1, 1);
srv(150, 90, 90, 30, 42, 33, 33, 42, 1, 3, 1, 1);
srv(120, 60, 180, 0, 42, 33, 6, 42, 1, 1, 3, 1);
srv(120, 60, 180, 0, 42, 33, 33, 24, 1, 1, 3, 1);
srv(90, 30, 150, 90, 42, 33, 33, 6, 1, 1, 1, 3);
srv(90, 30, 150, 90, 42, 33, 33, 42, 1, 1, 1, 3);
srv(180, 0, 120, 60, 6, 33, 33, 42, 3, 1, 1, 1);
srv(180, 0, 120, 60, 42, 33, 33, 33, 3, 1, 1, 1);
}
//== Right ======================================================
void turn_right ()
{
// set servo positions and speeds needed to turn right one step
// (LFP, LBP, RBP, RFP, LFL, LBL, RBL, RFL, S1, S2, S3, S4)
srv( 90, 30, 150, 90, 6, 33, 33, 42, 3, 1, 1, 1);
srv( 90, 30, 150, 90, 42, 33, 33, 42, 3, 1, 1, 1);
srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3);
srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3);
srv(150, 90, 90, 30, 42, 33, 6, 42, 1, 1, 3, 1);
srv(150, 90, 90, 30, 42, 33, 33, 42, 1, 1, 3, 1);
srv(180, 0, 120, 60, 42, 6, 33, 42, 1, 3, 1, 1);
srv(180, 0, 120, 60, 42, 33, 33, 42, 1, 3, 1, 1);
}
//== Center Servos ======================================================
void center_servos()
{
myServo1.write(90);
myServo2.write(90);
myServo3.write(90);
myServo4.write(90);
myServo5.write(90);
myServo6.write(90);
myServo7.write(90);
myServo8.write(90);
int s11 = 90; // Front Left Pivot Servo
int s12 = 90; // Front Left Lift Servo
int s21 = 90; // Back Left Pivot Servo
int s22 = 90; // Back Left Lift Servo
int s31 = 90; // Back Right Pivot Servo
int s32 = 90; // Back Right Lift Servo
int s41 = 90; // Front Right Pivot Servo
int s42 = 90; // Front Right Lift Servo
}
//== Increase Speed ======================================================
void increase_speed()
{
if (spd > 3)
spd--;
}
//== Decrease Speed ========================================================================
void decrease_speed()
{
if (spd < 50)
spd++;
}
//== Srv ======================================================
void srv( int p11, int p21, int p31, int p41, int p12, int p22, int p32, int p42, int sp1, int sp2, int sp3, int sp4)
{
// p11: Front Left Pivot Servo
// p21: Back Left Pivot Servo
// p31: Back Right Pivot Servo
// p41: Front Right Pivot Servo
// p12: Front Left Lift Servo
// p22: Back Left Lift Servo
// p32: Back Right Lift Servo
// p42: Front Right Lift Servo
// sp1: Speed 1
// sp2: Speed 2
// sp3: Speed 3
// sp4: Speed 4
// Multiply lift servo positions by manual height adjustment
p12 = p12 + high * 3;
p22 = p22 + high * 3;
p32 = p32 + high * 3;
p42 = p42 + high * 3;
while ((s11 != p11) || (s21 != p21) || (s31 != p31) || (s41 != p41) || (s12 != p12) || (s22 != p22) || (s32 != p32) || (s42 != p42))
{
// Front Left Pivot Servo
if (s11 < p11) // if servo position is less than programmed position
{
if ((s11 + sp1) <= p11)
s11 = s11 + sp1; // set servo position equal to servo position plus speed constant
else
s11 = p11;
}
if (s11 > p11) // if servo position is greater than programmed position
{
if ((s11 - sp1) >= p11)
s11 = s11 - sp1; // set servo position equal to servo position minus speed constant
else
s11 = p11;
}
// Back Left Pivot Servo
if (s21 < p21)
{
if ((s21 + sp2) <= p21)
s21 = s21 + sp2;
else
s21 = p21;
}
if (s21 > p21)
{
if ((s21 - sp2) >= p21)
s21 = s21 - sp2;
else
s21 = p21;
}
// Back Right Pivot Servo
if (s31 < p31)
{
if ((s31 + sp3) <= p31)
s31 = s31 + sp3;
else
s31 = p31;
}
if (s31 > p31)
{
if ((s31 - sp3) >= p31)
s31 = s31 - sp3;
else
s31 = p31;
}
// Front Right Pivot Servo
if (s41 < p41)
{
if ((s41 + sp4) <= p41)
s41 = s41 + sp4;
else
s41 = p41;
}
if (s41 > p41)
{
if ((s41 - sp4) >= p41)
s41 = s41 - sp4;
else
s41 = p41;
}
// Front Left Lift Servo
if (s12 < p12)
{
if ((s12 + sp1) <= p12)
s12 = s12 + sp1;
else
s12 = p12;
}
if (s12 > p12)
{
if ((s12 - sp1) >= p12)
s12 = s12 - sp1;
else
s12 = p12;
}
// Back Left Lift Servo
if (s22 < p22)
{
if ((s22 + sp2) <= p22)
s22 = s22 + sp2;
else
s22 = p22;
}
if (s22 > p22)
{
if ((s22 - sp2) >= p22)
s22 = s22 - sp2;
else
s22 = p22;
}
// Back Right Lift Servo
if (s32 < p32)
{
if ((s32 + sp3) <= p32)
s32 = s32 + sp3;
else
s32 = p32;
}
if (s32 > p32)
{
if ((s32 - sp3) >= p32)
s32 = s32 - sp3;
else
s32 = p32;
}
// Front Right Lift Servo
if (s42 < p42)
{
if ((s42 + sp4) <= p42)
s42 = s42 + sp4;
else
s42 = p42;
}
if (s42 > p42)
{
if ((s42 - sp4) >= p42)
s42 = s42 - sp4;
else
s42 = p42;
}
// Write Pivot Servo Values
myServo1.write(s11 + da);
myServo3.write(s21 + db);
myServo5.write(s31 + dc);
myServo7.write(s41 + dd);
// Write Lift Servos Values
myServo2.write(s12);
myServo4.write(s22);
myServo6.write(s32);
myServo8.write(s42);
delay(spd); // Delay before next movement
}//while
} //srv
//== USRF Function ======================================================
long get_distance(bool unit)
{
// if unit == 0 return inches, else return cm
long duration = 0,
cm = 0,
inches = 0;
// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
digitalWrite(trigPin, LOW);
delayMicroseconds(5);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the signal from the sensor: a HIGH pulse whose
// duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
// convert the time into a distance
cm = (duration / 2) / 29.1;
inches = (duration / 2) / 74;
if (unit == INCH)
return inches;
else
return cm;
}
---------------------------------
Información y ventas:
Bogotá, Colombia
Cel: 310 796 4779
contactomediatk@gmail.com
También puede comprar por mercadolibre, Colombia:
No hay comentarios:
Publicar un comentario