Codi del programa Arduino
Codi del programa Arduino
Embedded Files
// Iniciem les llibreries necessàries
#include <Stepper.h>
#include <Servo.h>
// Activem els servomotors
Servo servomotorBase; // Servomotor de la base
Servo servomotorArticulacio; // Servomotor de l'articulació
Servo servomotorPinça; // Servomotor de la pinça
const int PassosPerVolta = 2048; // Passos per fer una volta completa del motor pas a pas
Stepper motorPasAPas(PassosPerVolta, 7, 5, 6, 4); // Motor pas a pas: passos per volta i pins de control
// Definim els pins amb noms descriptius per fer-ho més fàcil en el futur
#define PIN_EIX_X_JOYSTICK A1 // Pin per a l'eix X del joystick
#define PIN_EIX_Y_JOYSTICK A0 // Pin per a l'eix Y del joystick
#define PIN_BOTO_SELECCIO 8 // Pin pel botó de selecció del joystick
#define PIN_OBRIR_PINÇA 13 // Pin pel botó d'obrir la pinça
#define PIN_TANCAR_PINÇA 12 // Pin pel botó de tancar la pinça
// Variables per guardar els valors dels eixos del joystick
int valorEixX = 0; // Valor de l'eix X
int valorEixY = 0; // Valor de l'eix Y
// Variables per les posicions dels servomotors
int posicioBase = 90; // Posició inicial de la base (centrada)
int posicioArticulacio = 90; // Posició inicial de l'articulació (centrada)
int posicioPinça = 90; // Posició inicial de la pinça (mig oberta)
// Variable per saber quin servo està seleccionat
int servoSeleccionat = 0; // 0: base, 1: articulació
// Variables per gestionar el polsador filtrar (el del joystik)
bool estatUltimBoto = HIGH; // Bool, perquè així sempre serà HIGH o LOW, 1 o 0
unsigned long tempsUltimFiltrat = 0;
unsigned long retardFiltrat = 50; // En mil·lisegons
void setup() {
Serial.begin(9600); // Inicialitzem la comunicació sèrie
servomotorBase.attach(9); // Assignem el pin 9 al servomotor de la base
servomotorArticulacio.attach(10); // Assignem el pin 10 al servomotor de l'articulació
servomotorPinça.attach(11); // Assignem el pin 11 al servomotor de la pinça
pinMode(PIN_BOTO_SELECCIO, INPUT); // Botó per seleccionar servo
pinMode(PIN_OBRIR_PINÇA, INPUT); // Botó per obrir la pinça
pinMode(PIN_TANCAR_PINÇA, INPUT); // Botó per tancar la pinça
motorPasAPas.setSpeed(10); // Velocitat del motor pas a pas
// Servos a la posició inicial
servomotorBase.write(posicioBase);
servomotorArticulacio.write(posicioArticulacio);
servomotorPinça.write(posicioPinça);
}
void loop() {
valorEixX = analogRead(PIN_EIX_X_JOYSTICK); // Llegim l'eix X del joystick
valorEixY = analogRead(PIN_EIX_Y_JOYSTICK); // Llegim l'eix Y del joystick
// GESTIÓ DEL POLSADOR FILTRAT
bool lecturaBoto = digitalRead(PIN_BOTO_SELECCIO);
if (lecturaBoto != estatUltimBoto) {
tempsUltimFiltrat = millis(); // Reiniciem el temporitzador de la filtració si canvia l'estat
}
if ((millis() - tempsUltimFiltrat) > retardFiltrat) {
if (lecturaBoto == LOW && estatUltimBoto == HIGH) { // Si s'ha premut el botó
servoSeleccionat = (servoSeleccionat + 1) % 2; // Canviem de base a articulació, o al revés.
Serial.print("Servo seleccionat: ");
Serial.println(servoSeleccionat + 1);
}
}
estatUltimBoto = lecturaBoto; // La senyal actual passa a ser l'antiga, i l'actual la llegirem del pi
// CONTROL DELS SERVOMOTORS
int zonaMorta = 40; // Zona morta per evitar petits moviments involuntaris amb el joystik
int increment = 2; // Augment petit per suavitzar el moviment
if (servoSeleccionat == 0) { // Si està seleccionada la base
if (valorEixY < 512 - zonaMorta) {
posicioBase = constrain(posicioBase + increment, 0, 180); // Incrementa gradualment el valor de la y entre 0 i 180.
servomotorBase.write(posicioBase); // El servomotor es mou.
} else if (valorEixY > 512 + zonaMorta) {
posicioBase = constrain(posicioBase - increment, 0, 180); // Decreix gradualment el valor de la y entre 0 i 180.
servomotorBase.write(posicioBase); // El servomotor es mou.
}
} else if (servoSeleccionat == 1) { // Si està seleccionada l'articulació
if (valorEixY < 512 - zonaMorta) {
posicioArticulacio = constrain(posicioArticulacio + increment, 0, 180); // Incrementa gradualment el valor de la y entre 0 i 180.
servomotorArticulacio.write(posicioArticulacio); // El servomotor es mou.
} else if (valorEixY > 512 + zonaMorta) {
posicioArticulacio = constrain(posicioArticulacio - increment, 0, 180); // Decreix gradualment el valor de la y entre 0 i 180.
servomotorArticulacio.write(posicioArticulacio); // El servomotor es mou.
}
}
// CONTROL DEL MOTOR PAS A PAS
int zonaMortaMotor = 40; // Zona morta per evitar petits moviments involuntaris amb el joystik com ho hem fet amb els servomotors.
int passosPerCicle = 10; // Pocs passos per tenir més control
if (valorEixX > 512 + zonaMortaMotor) {
motorPasAPas.step(passosPerCicle); // Gira en un sentit
} else if (valorEixX < 512 - zonaMortaMotor) {
motorPasAPas.step(-passosPerCicle); // Gira en l'altre sentit
}
// CONTROL DE LA PINÇA
int incrementPinça = 1; // Increment petit per suavitzar l'obertura/tancament
int pinçaOberta = 180; // Valor màxim d'obertura
int pinçaTancada = 0; // Valor mínim de tancament
// Botó per obrir la pinça
if (digitalRead(PIN_OBRIR_PINÇA) == HIGH) {
posicioPinça = constrain(posicioPinça + incrementPinça, pinçaTancada, pinçaOberta); //Incrementa gradualment l'obertura de la pinça entre 0 i 180.
servomotorPinça.write(posicioPinça); // El servomotor es mou i la pinça es va obrint.
delay(10); // Controla la velocitat d'obertura
}
// Botó per tancar la pinça
if (digitalRead(PIN_TANCAR_PINÇA) == HIGH) {
posicioPinça = constrain(posicioPinça - incrementPinça, pinçaTancada, pinçaOberta); //Decreix gradualment l'obertura de la pinça entre 0 i 180.
servomotorPinça.write(posicioPinça); // El servomotor es mou i la pinça es va tancant.
delay(10); // Controla la velocitat de tancament
}
// Ho hem anat mostrant tot per la consola per tenir control del que passa, que totes les variables funcionin, no donin error, i que tot funcioni bé.
Serial.print("x = ");
Serial.print(valorEixX);
Serial.print(", y = ");
Serial.print(valorEixY);
Serial.print(", Servo seleccionat: ");
Serial.print(servoSeleccionat + 1);
Serial.print(", Pinça: ");
Serial.println(posicioPinça);
}
Page updated
Report abuse