Untitled
unknown
plain_text
2 years ago
13 kB
5
Indexable
#include <Servo.h>
#include <SoftwareSerial.h>
SoftwareSerial Bluetooth(3, 4); // RX, TX
Servo servo1; //BASE
Servo servo2; //HOMBRO1
Servo servo3; //HOMBRO2 no se usa en este proyecto, la idea principal era usar 2 servos para HOMBRO
Servo servo4; //CODO1
Servo servo5; //CODO2
Servo servo6; //MUÑECA
Servo servo7; //GRIPPER
//10rapido 15medio 20lento
int s1Vel = 15; //BASE
int s2Vel = 25; //HOMBRO
int s4Vel = 20; //CODO1
int s5Vel = 20; //CODO2
int s6Vel = 15; //MUÑECA
int s7Vel = 15; //GRIPPER
int index=0;
int velG=25;
int s1Act,s2Act,s3Act,s4Act,s5Act,s6Act,s7Act; //posicion actual de los servos
int s1Ant,s2Ant,s3Ant,s4Ant,s5Ant,s6Ant,s7Ant; //posicion anterior de los servos
int s1[50],s2[50],s3[50],s4[50],s5[50],s6[50],s7[50]; //matrices para almacenas las posiciones de cada servo
String bt,btS;
void setup(){
Serial.begin(115200);
Bluetooth.begin(9600);
Bluetooth.setTimeout(10);
servo1.attach(5,510,1200); //ancho min y max exclusivo para MG996R base
servo2.attach(11,650,1400); //ancho min y max exclusivo para MG946R hombro1
servo3.attach(6,650,1400); //ancho min y max exclusivo para MG946R hombro2
servo4.attach(7,650,1400); //ancho min y max exclusivo para MG946R codo1
servo5.attach(8,650,1400); //ancho min y max exclusivo para MG995 codo2
servo6.attach(9,800,1290); //ancho min y max exclusivo para MG995 muneca
servo7.attach(10,700,1290); //ancho min y max exclusivo para MG995 gripper
s1Ant=90; //BASE
servo1.write(s1Ant);
//Importante empezar s2 y s3 complementados a 180°;es decir; s2+s3=180;
s2Ant=100; //HOMBRO1
s3Ant=80; //HOMBRO2
servo2.write(s2Ant);
servo3.write(s3Ant);
s4Ant=115; //CODO1
servo4.write(s4Ant);
s5Ant=60; //CODO2
servo5.write(s5Ant);
s6Ant=90; //MUÑECA
servo6.write(s6Ant);
s7Ant=60; //GRIPPER
servo7.write(s7Ant);
delay(50);
}
void loop() {
if(Bluetooth.available()>0){
bt = Bluetooth.readString(); // leemos cadena
//////////////////////////// SERVO 1 - BASE /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s1")){ // comprueba si la cadena comienza con "s1"
btS = bt.substring(2,bt.length()); // extraigo los caracteres desde la poisicion 2 en adelante ejem:"s1120" a "120"
s1Act = btS.toInt(); //convierto de string a entero
//MUEVO EL SERVOMOTOR CON UN BARRIDO
if(s1Ant > s1Act){
for(int j=s1Ant; j>=s1Act; j--){
servo1.write(j);
delay(s1Vel);}}
else{
for(int j=s1Ant; j<=s1Act; j++){
servo1.write(j);
delay(s1Vel);}}
s1Ant = s1Act;} //el angulo movido(s1Act) se convierte en la posicion anterior
//////////////////////////// SERVO 2 y 3 - HOMBRO 1 y 2 /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s2")){
btS = bt.substring(2,bt.length());
s2Act = btS.toInt();
Serial.println(s2Act);
if(s2Ant > s2Act){
for(int j=s2Ant; j>=s2Act; j--){
servo2.write(j);
servo3.write(180-j);
delay(s2Vel);}}
else{
for(int j=s2Ant; j<=s2Act; j++){
servo2.write(j);
servo3.write(180-j);
delay(s2Vel);}}
s2Ant = s2Act;}
//////////////////////////// SERVO 4 - CODO1 /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s4")){
btS = bt.substring(2,bt.length());
s4Act = btS.toInt();
if(s4Ant > s4Act){
for(int j=s4Ant; j>=s4Act; j--){
servo4.write(j);
delay(s4Vel);}}
else{
for(int j=s4Ant; j<=s4Act; j++){
servo4.write(j);
delay(s4Vel);}}
s4Ant = s4Act;}
//////////////////////////// SERVO 5 - CODO2 /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s5")){
btS = bt.substring(2,bt.length());
s5Act = btS.toInt();
if(s5Ant > s5Act){
for(int j=s5Ant; j>=s5Act; j--){
servo5.write(j);
delay(s5Vel);}}
else{
for(int j=s5Ant; j<=s5Act; j++){
servo5.write(j);
delay(s5Vel);}}
s5Ant = s5Act;}
//////////////////////////// SERVO 6 - MUÑECA /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s6")){
btS = bt.substring(2,bt.length());
s6Act = btS.toInt();
Serial.println(s6Act);
if(s6Ant > s6Act){
for(int j=s6Ant; j>=s6Act; j--){
servo6.write(j);
delay(s6Vel);}}
else{
for(int j=s6Ant; j<=s6Act; j++){
servo6.write(j);
delay(s6Vel);}}
s6Ant = s6Act;}
//////////////////////////// SERVO 7 - GRIPPER /////////////////////////////////////////////////////////////////////////////////
if(bt.startsWith("s7")){
btS = bt.substring(2,bt.length());
s7Act = btS.toInt();
if(s7Ant > s7Act){
for(int j=s7Ant; j>=s7Act; j--){
servo7.write(j);
delay(s7Vel);}}
else{
for(int j=s7Ant; j<=s7Act; j++){
servo7.write(j);
delay(s7Vel);}}
s7Ant = s7Act;}
//////////////////////////// SAVE //////////////////////////////////////////////////
if(bt.startsWith("SAVE")){
s1[index]= s1Ant;
s2[index]= s2Ant;
s4[index]= s4Ant;
s5[index]= s5Ant;
s6[index]= s6Ant;
s7[index]= s7Ant;
index++;}
///////////////////////// RESET ///////////////////////////////////////////////////
if(bt.startsWith("RESET")){
//memset se utiliza para rellenar un bloque de memoria con un valor determinado
//memeset("matriz","dato a rellenar","tamaño de la matriz")
memset(s1,0,sizeof(s1)); //rellena con el valor de "0" todas las posiciones de la matriz
memset(s2,0,sizeof(s2));
memset(s4,0,sizeof(s4));
memset(s5,0,sizeof(s5));
memset(s6,0,sizeof(s6));
memset(s7,0,sizeof(s7));
index=0;}
//////////////////////////// RUN ////////////////////////////////////////////////////
if(bt.startsWith("RUN")){
//Ejecuta los pasos una y otra vez hasta que se pulse el botón "RESET" o espera hasta que se vuelva a pulsar "RESET"
while(bt.startsWith("RESET") != true){//MIENTRAS NO SE PRESIONE 'RESET' SE MANTIENE EN EL BUCLE y SOLO FUNCIONA BOTON "PAUSE"
for(int i=0; i<= index-2;i++){
if(Bluetooth.available()>0){
bt = Bluetooth.readString();
if(bt.startsWith("PAUSE") == true){
//Ejecuta los pasos una y otra vez hasta que se pulse el botón "RUN" o espera hasta que se vuelva a pulsar "RUN"
while(bt.startsWith("RUN") != true){//MIENTRAS NO SE PRESIONE 'RUN' SE MANTIENE EN EL BUCLE Y SOLO FUNCIONA BOTON "RESET"
if(Bluetooth.available()>0){
bt = Bluetooth.readString();
if(bt.startsWith("RESET") == true){
break;}
}
}
}//end if(PAUSE)
if(bt.startsWith("ss")){
btS = bt.substring(2,bt.length());
velG = btS.toInt();}
}//end 1er Bluetooth.available
//////// SERVO 1 ////////////////////////
if(s1[i] == s1[i+1]){}
if(s1[i] > s1[i+1]){
for(int j = s1[i]; j >= s1[i+1];j--){
servo1.write(j);
delay(velG);}}
if(s1[i] < s1[i+1]){
for(int j = s1[i]; j <= s1[i+1];j++){
servo1.write(j);
delay(velG);}}
//////// SERVO 2 y 3 /////////////////////
if(s2[i] == s2[i+1]){}
if(s2[i] > s2[i+1]){
for(int j = s2[i]; j >= s2[i+1];j--){
servo2.write(j);
servo3.write(180-j);
delay(velG);}}
if(s2[i] < s2[i+1]){
for(int j = s2[i]; j <= s2[i+1];j++){
servo2.write(j);
servo3.write(180-j);
delay(velG);}}
//////// SERVO 4 ////////////////////////
if(s4[i] == s4[i+1]){}
if(s4[i] > s4[i+1]){
for(int j = s4[i]; j >= s4[i+1];j--){
servo4.write(j);
delay(velG);}}
if(s4[i] < s4[i+1]){
for(int j = s4[i]; j <= s4[i+1];j++){
servo4.write(j);
delay(velG);}}
//////// SERVO 5 ////////////////////////
if(s5[i] == s5[i+1]){}
if(s5[i] > s5[i+1]){
for(int j = s5[i]; j >= s5[i+1];j--){
servo5.write(j);
delay(velG);}}
if(s5[i] < s5[i+1]){
for(int j = s5[i]; j <= s5[i+1];j++){
servo5.write(j);
delay(velG);}}
//////// SERVO 6 ////////////////////////
if(s6[i] == s6[i+1]){}
if(s6[i] > s6[i+1]){
for(int j = s6[i]; j >= s6[i+1];j--){
servo6.write(j);
delay(velG);}}
if(s6[i] < s6[i+1]){
for(int j = s6[i]; j <= s6[i+1];j++){
servo6.write(j);
delay(velG);}}
//////// SERVO 7 ////////////////////////
if(s7[i] == s7[i+1]){}
if(s7[i] > s7[i+1]){
for(int j = s7[i]; j >= s7[i+1];j--){
servo7.write(j);
delay(velG);}}
if(s7[i] < s7[i+1]){
for(int j = s7[i]; j <= s7[i+1];j++){
servo7.write(j);
delay(velG);}}
}// end FOR INDEX
}//end 1er WHILE "RESET"
}
}//if(Bluetooth.available()>0){
}//end void loopEditor is loading...