Untitled

mail@pastecode.io avatar
unknown
plain_text
a year ago
13 kB
1
Indexable
Never
#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 loop