segunda-feira, 27 de fevereiro de 2017

Braço robótico programável




Esquema de ligações.
Código


//Projeto de construção de um braço robótico programável 6 OU 7 DOF
//para fins de estudos
//por José de Sousa Araújo
//_____________________________________________________________________________________________________

//1º  incluir as bibliotecas
#include <Servo.h>              //Biblioteca do servo
#include <EEPROM.h>             // Biblioteca de EEPROM
#include <Wire.h>               //Biblioteca do Módulo IC para o Display de Crystal Líquido
#include <LiquidCrystal_I2C.h>  // Biblioteca do Módulo Display de Crystal Líquido

//Inicializando o display no endereço 0x27
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

//declarando as variáveis para os servos motores
Servo Base;     
Servo Ombro;
Servo Cotovelo;
Servo Punho1;
Servo Punho2;
Servo Garra;
Servo Garra2;

//setanto as variáveis declaradas como inteiro nas portas

int state = LOW;    //state1 definido como baixo por padrão inicial
int state1 = LOW;   //state1 definido como baixo por padrão inicial

int stgr;
int btnGravar = 8;  //Botão para gravar a posição dos servos ligado a porta 8 digital
int btnExecutar = 9;//Botão para executar os movimentos gravados na EEPRON ligado a porta digital 9
int btnGarra = 10;  //Botão para abrir e fechar a garra. Ligado a porta 10 digital
int buzzer = 11;    //Buzzer para dar bip ligado a porta digital 11

int potbase = 0;    //Canal Analógico A0
int potombro = 1;   //Canal Analógico A1
int potcot = 2;     //Canal Analógico A2
int potPunho1 = 3;  //Canal Analógico A3
int potPunho2 = 6;  //Canal Analógico A6

int sensor = 7;     //Canal Analógico para o sensor

int valbase;    // Constante p/ valor do ângulo servomotor Base
int valombro;   // Constante p/ valor do ângulo servomotor Ombro
int valcot;     // Constante p/ valor do ângulo servomotor Cotovelo
int valPunho1;  // Constante p/ valor do ângulo servomotor Punho1
int valPunho2;  // Constante p/ valor do ângulo servomotor Punho2 - ROTACIONAR O PUNHO
int valGarra;   // Constante p/ valor do ângulo servomotor Garra
int valGarra2;   // Constante p/ valor do ângulo servomotor Garra

int varGarra=0;       // valor instantaneo enviado pelo botão da garra
int varGarra2=0;     // valor guardado para estado da garra
int estado=0;

int addr = 0;    // Endereço na EEPRON de escrita na memória
int cont;
int SNSR; //Variável para a leitura do SENSOR
int address = 0;    // endereço na EEPRON de leitura na memória
byte value;    // valor lido da EEPROM


void setup()
{
 //iniciando o display com 16 colunas e duas linhas 
  lcd.begin (16, 2);
  lcd.clear(); //limpando qualquer valor escrito no display
  lcd.setBacklight(HIGH);//iluminando o display
  lcd.setCursor(4, 0); //iniciando na linha 0 e coluna 0
  lcd.print("PROGRAMAR"); //escrevendo a palavra programar na primenira linha e braço robótico na segunda linha
  lcd.setCursor(0, 1);
  lcd.print("Braco Robotico");
  delay(1500);
  lcd.clear();

  pinMode(btnGravar, INPUT);//informando que a botão gravar é uma entrada
  pinMode(btnExecutar, INPUT);//botão executar como entrada
  pinMode(btnGarra, INPUT);//botão garra como entrada
  pinMode(buzzer, OUTPUT);//buzzer como saída

  Base.attach(2);
  Ombro.attach(3);
  Cotovelo.attach(4);
  Punho1.attach(5);
  Punho2.attach(6);
  Garra.attach(7);
  Garra2.attach(12);
  Serial.begin(9600);
}


void loop() {
  
  state = digitalRead(btnGravar); // Verifica o status do botão. Se for pressionado grava a posição atual dos servos
  if (state == HIGH) {
    digitalWrite(buzzer, HIGH); //Liga sinal de aviso do buzzer
    delay(200);
    digitalWrite(buzzer, LOW); //desliga sinal de aviso do buzzer
    delay(50);
    

    valbase = analogRead(potbase); // Lê e o valor do potenciometro
    valbase = map(valbase, 0, 1023, 0, 180); // Converte o valor lido p/ respectiva     referência entre (0° a 180°)
    EEPROM.write(addr, valbase); // Grava o valor lido na EEPROM
    addr = addr + 1; //Incremento p/ o proximo endereço
    delay(50);


    valombro = analogRead(potombro); //
    valombro = map(valombro, 0, 1023, 60, 180);
    EEPROM.write(addr, valombro);
    addr = addr + 1;
    delay(50);

    valcot = analogRead(potcot);
    valcot = map(valcot, 0, 1023, 0, 180);
    EEPROM.write(addr, valcot);
    addr = addr + 1;
    delay(50);

    valPunho1 = analogRead(potPunho1);
    valPunho1 = map(valPunho1, 0, 1023, 5, 175);
    EEPROM.write(addr, valPunho1);
    addr = addr + 1;
    delay(50);

    valPunho2 = analogRead(potPunho2);
    valPunho2 = map(valPunho2, 0, 1023, 0, 180);
    EEPROM.write(addr, valPunho2);
    addr = addr + 1;
    delay(50);

    EEPROM.write(addr, valGarra);
    addr = addr + 1;
    delay(50);
    
    EEPROM.write(addr, valGarra2);
    addr = addr + 1;
    delay(50);

    if (addr == 1024) { // Aviso para limite de posições salvas na EEPROM
      delay(10);
      digitalWrite(buzzer, HIGH);
      delay(10);
      digitalWrite(buzzer, LOW);
      delay(10);
      digitalWrite(buzzer, HIGH);
      delay(10);
      digitalWrite(buzzer, LOW);
      delay(10);
    }
  }

  else {

    //leitura do analogico
 //    state1 = digitalRead(btnExecutar);
    
//
 ///////////////// USANDO O BOTAO PARA FECHAR E ABRIR A GARRA
    //stategarra = digitalRead(btnGravar);
        varGarra=digitalRead(btnGarra); // ler o valor enviado pelo botão: "HIGH" ou "LOW"
      if ((varGarra == HIGH) && (varGarra2 == LOW)) {
        estado = 1 - estado;
        delay(20); // delay para evitar varios acionamentos da garra ao manter pressionado
      }
        varGarra2=varGarra;
        if (estado == 1) {
          valGarra = (100);
          valGarra2 = (80);
          Garra.write(100); // abre a garra
          Garra2.write(80); // abre a garra
          lcd.setCursor(15, 1);
          lcd.print("A"); 
      }  else {
          valGarra = (80);
          valGarra2 = (115);
          Garra.write(80);  // fecha a garra
          Garra2.write(115);  // fecha a garra
          lcd.setCursor(15, 1);
          lcd.print("F"); 
      }
    

/////////////////////////////////////////////////////////////
    valbase = analogRead(potbase); //Lê o valor do potenciometro de ( 0 - 1023)
    valbase = map(valbase, 0, 1023, 0, 180); //converte o valor lido de ( 0 - 1023) p/ (0 °– 180°)
    Base.write(valbase);

    valombro = analogRead(potombro);
    valombro = map(valombro, 0, 1023, 60, 180);
    Ombro.write(valombro);
        
    valcot = analogRead(potcot);
    valcot = map(valcot, 0, 1023, 0, 180);
    Cotovelo.write(valcot);
    
    valPunho1 = analogRead(potPunho1);
    valPunho1 = map(valPunho1, 0, 1023, 5, 175);
    Punho1.write(valPunho1);

    valPunho2 = analogRead(potPunho2);
    valPunho2 = map(valPunho2, 0, 1023, 0, 180);
    Punho2.write(valPunho2);

  }
        lcd.setCursor(0, 0);
        lcd.print("B=");
        lcd.setCursor(2, 0);
        lcd.print("   ");
        lcd.setCursor(2, 0);
        lcd.print(valbase);
        lcd.setCursor(6, 0);
        lcd.print("O=");
        lcd.setCursor(8, 0);
        lcd.print("   ");
        lcd.setCursor(8, 0);
        lcd.print(valombro);
        
        lcd.setCursor(0, 1);
        lcd.print("C=");
        lcd.setCursor(2, 1);
        lcd.print("   ");
        lcd.setCursor(2, 1);
        lcd.print(valcot);
        lcd.setCursor(6, 1);
        lcd.print("P2=");
        lcd.setCursor(9, 1);
        lcd.print("   ");
        lcd.setCursor(9, 1);
        lcd.print(valPunho2); 
  //}
  state1 = digitalRead(btnExecutar); // Botão p/ acionamento das movimentações gravadas
  if (state1 == HIGH) {
    digitalWrite(buzzer, HIGH);
    delay(100);
    digitalWrite(buzzer, LOW);
    delay(100);
    digitalWrite(buzzer, HIGH);
    delay(100);
    digitalWrite(buzzer, LOW);
    delay(100);
    Play(); //Chamada da função p/ movimentar
  }
}

void movServo(Servo &s, int ANGfinal, int veloc) {
  int ANGinicial = s.read(); // lê a posição atual do servo e o coloca como o angulo inicial de movimento
  if (ANGinicial > ANGfinal) {
    for (int i = ANGinicial; i > ANGfinal; i--) { //decremanta 1º grau
      s.write(i); // 'escreve' o valor 'i' no servo selecionado
      delay(veloc); // tempo de espera.
    }
  }
  else {
    for (int i = ANGinicial; i <= ANGfinal; i++) { //incremanta 1º grau
      s.write(i); // 'escreve' o valor 'i' no servo selecionado
      delay(veloc); // tempo de espera.
    }
  }
}
void Play() {
  delay(500);
  state1 = digitalRead(btnExecutar);
  while (state1 == LOW) {
   int l;
    SNSR = 0; //Zera a variável SNSR
    for (l = 0; l < 10; l++) { //Faz 10 vezes a leitura do sensor
      cont = analogRead(sensor); //Lê o valor do sensor e guarda na variável SNSR
      SNSR = SNSR + cont; //Armazenamento na varíavel SNSR o valor lido + o valor anterior
      delay(10); //Delay se 10 milissegundos
    }
    SNSR = SNSR / 10; //Divide o valor armazenado por 10
//    Serial.println(SNSR); //Imprime o valor do SNSR

    if (SNSR > 100) { // Se o valor do SNSR for maior a 100
      movServo(Base, valbase, 90);
      movServo(Ombro, valombro, 90);
      movServo(Cotovelo, valcot, 90);
      movServo(Punho1, valPunho1, 90);
      movServo(Punho2, valPunho2, 90);
      movServo(Garra, valGarra, 100);
      movServo(Garra2, valGarra2, 80);
      
      lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("SENSORES");
          lcd.setCursor(0, 1);
          lcd.print("AGUARDANDO");
          lcd.setCursor(12, 1);
          lcd.print(SNSR);        

    }
    if (SNSR < 100) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("EXECUTANDO");
          lcd.setCursor(0, 1);
          lcd.print("PROGRAMA GRAVADO");
        
      
      int j;
      address = 0;
      addr = addr - 1;
      Serial.println("addr");
      Serial.println(addr);

      for (j = 0; j < addr; j += 7) {

        value = EEPROM.read(address); // Leitura da EEPROM
        movServo(Base, value, 15);
        delay(50);
        address = address + 1;
        delay(50);

        value = EEPROM.read(address);
        movServo(Ombro, value, 15);
        delay(50);
        address = address + 1;
        delay(50);

        value = EEPROM.read(address);
        movServo(Cotovelo, value, 15);
        delay(50);
        address = address + 1;
        delay(50);

        value = EEPROM.read(address);
        movServo(Punho1, value, 15);
        delay(50);
        address = address + 1;
        delay(50);

        value = EEPROM.read(address);
        movServo(Punho2, value, 15);
        delay(50);
        address = address + 1;
        delay(50);

        value = EEPROM.read(address);
        movServo(Garra, value, 15);
        delay(10);
        address = address + 1;
        //delay(10);

        
        value = EEPROM.read(address);
        movServo(Garra2, value, 15);
        delay(10);
        address = address + 1;
        delay(10);
        
  //      value = EEPROM.read(address);
  //      movServo(Garra, value, 15);
  //      delay(50);
  //      address = address + 1;
  //      delay(50);


      }

    }
  }
}