Brazo robot con Servo motores (5 / 6 paso)

Paso 5: Enchufe y escribir algo de código

Ahora, buscar a través del código y asegúrese de que su brazo robótico está conectado correctamente (prestar atención a los pines del potenciómetro). Tenía una fuente de alimentación de sobremesa por cable al blindaje del motor por lo que el escudo estaba recibiendo 6V en hasta 3A, entonces el Arduino solo fue alimentado a través del cable USB.  Los dos cables de cada motor están conectados a los bloques de M3 y M4 en el escudo del motor.  De los tres cables del potenciómetro de cada motor, uno va a tierra, el otro va a 5V de alimentación en el Arduino y el otro es un cable de señal que es conectado a una entrada de Arduino analógica. No se ser de soldadura importa qué configuración ir con.

Ahora, copie y pegue este código muy bien comentado:

/*
Control del brazo robot mediante servos hackeados
y es vertical + x está a la derecha
por Roberts Dustyn 2012-05
*/

#include < AFMotor.h >
#include < math.h >
#include < PID_v1.h >

Crear instancias de dos motores
Shoulder(3) de AF_DCMotor;
Elbow(4) de AF_DCMotor;

declarar pines
int ElbowPin = A1;  potenciómetro en el motor del codo
int ShoulderPin = A0; potenciómetro en el motor del hombro

INICIALIZAR CONSTANTES
doble Kp_Elbow = 20;    Esta es la ganancia proporcional
doble Kp_Shoulder = 20;
doble Ki_Elbow = 0.1;    Esta es la ganancia integral
doble Ki_Shoulder = 0.1;
doble Kd_Elbow = 1.0;  Esta es la ganancia derivativa
doble Kd_Shoulder = 0.75;

doble Elbow_neg = 970;      límites de conjuntos de brazo robótico utilizando la regla de la mano derecha para la muestra
doble Elbow_pos = 31;
doble Shoulder_neg = 210;
doble Shoulder_pos = 793;

const doble a1 = 200;     longitud de hombro a codo "hueso" (mm)
const doble a2 = 220;     longitud del codo a la muñeca "hueso" (mm) - largo soporte c

doble highY = 350; objetivos de línea de dibujo
doble lowY = 250;
constantX doble = 200;

Boolean elbowup = false; True = el codo hacia arriba, false = codo abajo

VARIABLES
doble rawElbowAngle = 0.0;      inicializar todos los ángulos 0
doble rawShoulderAngle = 0.0;

doble elbowAngle = 0.0;      inicializar todos los ángulos 0
doble shoulderAngle = 0.0;

theta1 doble = 0.0;  ángulos de objetivo determinado a través de IK
doble theta2 = 0.0;

actualX doble = 0.0;
Actualmente doble = 0.0;
doble y = 0.0;

doble c2 = 0.0; es btwn -1 y 1
doble s2 = 0.0;

doble pwmTemp = 0.0;

doble pwmElbow = 100.0;  entre 0 y 255
doble pwmShoulder = 100.0;

Sintaxis: PID (y entrada y salida y Setpoint, Kp, Ki, Kd, dirección)
PID elbowPID (& elbowAngle & pwmElbow & theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, directo);
PID shoulderPID (& shoulderAngle & pwmShoulder & theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, directo);

void setup() {}
Serial.Begin(115200);           Serie biblioteca

elbowPID.SetSampleTime(5);  (ms) con qué frecuencia nuevo PID calc se realiza, por defecto es 1000
shoulderPID.SetSampleTime(5);

elbow.setSpeed(pwmElbow);     ajustar la velocidad a pwmElbow
shoulder.setSpeed(pwmShoulder);     ajustar la velocidad a pwmElbow

elbowPID.SetMode(AUTOMATIC);
shoulderPID.SetMode(AUTOMATIC);

elbowPID.SetOutputLimits(-255,255);
shoulderPID.SetOutputLimits(-255,255);

move_to_start();  llegar a la posición inicial
}

void loop() {}
line_y();
}

void move_to_start() {}
{}
get_angles (constantX, lowY);
drive_joints();  las articulaciones de la unidad hasta que es igual real esperado
}
mientras (abs (theta1 - shoulderAngle) > 2 & & abs (theta2 - elbowAngle) > 2); fianza cuando está cerca
}

void line_y() {}
para (y = lowY; y < highY; y +=.5) {/ / establecer línea recta
get_angles(constantX,y);
drive_joints();
}
para (y = highY; y > lowY; y-= 5) {/ / dibujar la línea recta hacia abajo
get_angles (constantX, y);
drive_joints();
}
}

Dado theta1, theta2 resolver objetivo (Px, Py) (cinemática delantera)
void get_xy() {}
actualX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
Actualmente = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
}

Dada de la destino (Px, Py) resolver theta1, theta2 (cinemática inversa)
void get_angles (doble Px, doble Py) {}
en primer lugar encontrar theta2 donde c2 = cos(theta2) y s2 = sin(theta2)
C2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); es btwn -1 y 1

Si (elbowup == false) {}
S2 = sqrt (1 - pow(c2,2));  raiz puede ser + o -, y cada una corresponde a una orientación diferente
}
else if (elbowup == true) {}
S2 = - sqrt (1 - pow(c2,2));
}
theta2 = degrees(atan2(s2,c2));  resuelve por el ángulo en grados y lugares en el cuadrante correcto

Ahora encuentra theta1 donde c1 = cos(theta1) y s1 = sin(theta1)
theta1 = grados (atan2 (-a2 * s2 * Px + (a1 + a2 * c2) * Py, (a1 + a2 * c2) * Px + a2 * s2 * Py));
}

void drive_joints() {}
leer los valores reales de todas las macetas
rawElbowAngle = analogRead(ElbowPin);
rawShoulderAngle = analogRead(ShoulderPin);

restringir el brazo robot ignorar de valores rango
elbowAngle = restringir (rawElbowAngle, Elbow_pos, Elbow_neg);
shoulderAngle = restringir (rawShoulderAngle, Shoulder_neg, Shoulder_pos);

mapa ahora los ángulos por lo que corresponden correctamente
elbowAngle = mapa (elbowAngle, Elbow_neg, Elbow_pos,-110.0, 85.0);
shoulderAngle = mapa (shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);

elbowPID.Compute();
shoulderPID.Compute();

UNIDAD de codo: Es CW, CCW es al revés
pwmTemp = abs(pwmElbow);
elbow.setSpeed(pwmTemp);     ahora uso la nueva salida del PID para establecer la velocidad

Si (pwmElbow < 0) {}
Elbow.Run(Forward);      convertirlo
}
else if (pwmElbow > 0) {}
Elbow.Run(backward);      convertirlo
}
elbow.run(RELEASE) otra cosa;      dejó de

UNIDAD de hombro: Es CCW, CW es hacia atrás
pwmTemp = abs(pwmShoulder);
shoulder.setSpeed(pwmTemp);     ajustar la velocidad

Si (pwmShoulder < 0) {}
shoulder.Run(backward);      convertirlo
}
else if (pwmShoulder > 0) {}
shoulder.Run(Forward);      convertirlo
}
shoulder.run(RELEASE) otra cosa;      dejó de

get_xy();
serie imprimir datos aquí si lo desea
Serial.Print(actualX);
Serial.Print (",");
Serial.println(actualY);
}