rvilela37

Dom Out 16, 2022 5:51 pm
Servos lentos com a ultilizaçao de painel OLED

Eae gente, sou inicante no arduino e estou com um projeto de um braço robotico, mas td vez que eu coloco o comando  testdrawbitmap(); para acender o display de acordo com a posiçao do analogico, os servos funcionam de maneita mt lenta e irresponsiva.
Segue o meu codigo abaixo :

#include "VarSpeedServo.h"
#include <Wire.h> //INCLUSÃO DE BIBLIOTECA
#include <Adafruit_GFX.h> //INCLUSÃO DE BIBLIOTECA
#include <Adafruit_SSD1306.h> //INCLUSÃO DE BIBLIOTECA
#include <SPI.h>
#define SCREEN_WIDTH 128 // Largura display, em pixels
#define SCREEN_HEIGHT 32 // Altura display, em pixels
#define OLED_RESET  -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define LOGO_HEIGHT   32
#define LOGO_WIDTH  128

const unsigned char seta_cimaseta_cima [] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x7c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf1, 0x8f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x7c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0, 0x0f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
const int seta_cimaallArray_LEN = 1;
const unsigned char* seta_cimaallArray[1] = {
  seta_cimaseta_cima
};

VarSpeedServo servo1;//Base
VarSpeedServo servo2;//Extensão
VarSpeedServo servo3;//Altura
VarSpeedServo servo4;//Garra

//Joystick Esquerdo
int potpin1 = A0;//VRy
int potpin2 = A1;//VRx
//Joystick Direito
int potpin3 = A2;//VRx
int potpin4 = A3;//VRy

int val1;
int val2;
int val3;
int val4;
int botaod = 12;
int botaoe = 13;
int estadod;
int estadoe;

int LED_vermelho = 7;
int LED_verde = 5;
int LED_azul = 4;
int ValorVermelho =  0;
int ValorVerde = 0;
int ValorAzul = 0;
int SOM = 3;
int numColors = 255;
int animationDelay = 10;
int counter = 0;


//Valores iniciais de posição de cada servo
static int s1 = 70;
static int s2 = 110;
static int s3 = 100;
static int s4 = 80;
void setup()
{
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // 0x3C sendo o end I2C
    Serial.println(F("Falha alocação SSD1306"));
    for (;; // Não processado, loop infinito
  }
    ValorVerde = 50;
  ValorVermelho = 50;
  ValorAzul = 50;
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_azul, ValorAzul);
  // Limpa display
  display.clearDisplay();
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.clearDisplay();
  display.setCursor(10, 10);
  display.print("Projeto Integrador        PUCCAMP");
  display.display();
  delay(2000);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(" Rafael Vilela \n Rodrigo OLiveira \n Murilo Caus \n Bruno Renzo");
  display.display();
  delay(3000);
  display.clearDisplay();
  display.print("");
  display.display();
  Serial.begin(38400);
  pinMode (botaod, INPUT_PULLUP);
  pinMode (botaoe, INPUT_PULLUP);
  pinMode(LED_azul, OUTPUT);
  pinMode (SOM, OUTPUT);
  pinMode(LED_vermelho, OUTPUT);
  pinMode(LED_verde, OUTPUT);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_azul, ValorAzul);
  servo1.attach(11); //Base, pino digital
  servo2.attach(10); //Extensão, pino digital
  servo3.attach(9); //Altura, pino digital
  servo4.attach(6); //Garra, pino digital
  //Move todo o braco para posicao inicial
  servo1.write(70);//Base
  servo2.write(110);//Extensão
  servo3.write(100);//Altura
  servo4.write(80);//Garra

}
void testdrawbitmap(void) {
  // Limpa display
  display.clearDisplay();

  // Preenche tela com bitmap
  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    seta_cimaseta_cima, LOGO_WIDTH, LOGO_HEIGHT, 1);
  // Exibe no display
  display.display();
  // Aguarda 1 s
  delay(1000);
}
void loop()
{
  estadod = digitalRead(botaod);
  estadoe = digitalRead(botaoe);
  if (!estadod && !estadoe) {
     ValorVerde = 0;
  ValorVermelho = 255;
  ValorAzul = 0;
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_azul, ValorAzul);
  delay (200);
    display.setCursor(18, 14);
    display.print("Resetando Eixos \n           \2");
    display.display();
    delay(2000);
    display.clearDisplay();
    display.print("");
    display.display();
  ValorVerde = 0;
  ValorVermelho = 255;
  ValorAzul = 255;
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_azul, ValorAzul);
  delay (200);
  digitalWrite (LED_vermelho, LOW);
  digitalWrite (LED_verde, LOW);
  digitalWrite (LED_azul, LOW);
  delay (200);
  ValorVerde = 0;
  ValorVermelho = 255;
  ValorAzul = 255;
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_azul, ValorAzul);
  delay (200);
  digitalWrite (LED_vermelho, LOW);
  digitalWrite (LED_verde, LOW);
  digitalWrite (LED_azul, LOW);
  delay (200);
  ValorVerde = 0;
  ValorVermelho = 255;
  ValorAzul = 255;
  analogWrite(LED_verde, ValorVerde);
  analogWrite(LED_vermelho, ValorVermelho);
  analogWrite(LED_azul, ValorAzul);
  delay (200);
  digitalWrite (LED_vermelho, LOW);
  digitalWrite (LED_verde, LOW);
  digitalWrite (LED_azul, LOW);
  tone(3, 15, 1000);
  servo1.write(70);//Base
  servo2.write(110);//Extensão
  servo3.write(100);//Altura
  servo4.write(80);//Garra
}
//Controle da base do braço
val1 = analogRead(potpin1);
//Para direita
if (val1 < 100)
{
  s1 = s1 - 2;
  if (s1 <= 10)
  {
    s1 = 10;
  }
  servo1.write(s1);
  delay(5);
}

//Para esquerda
if (val1 > 900)
{
  s1 = s1 + 2;//soma
  if (s1 >= 170)
  {
    s1 = 170;
  }
  testdrawbitmap();
  display.clearDisplay();
  servo1.write(s1);
  delay(5);
}
else {
 delay (50);
  display.clearDisplay();
  display.print("");
  display.display();
}

// Controle da extensão do braço
val2 = analogRead(potpin2);
//Para trás
if (val2 > 900)
{
  s2 = s2 - 2;
  if (s2 <= 10)
  {
    s2 = 10;
  }
  servo2.write(s2);
  delay(5);
}

//Para frente
if (val2 < 100)
{
  s2 = s2 + 2;
  if (s2 >= 170)
  {
    s2 = 170;
  }
  servo2.write(s2);
  delay(5);
}


// Controle da altura do braço
//Abaixar o braço
val3 = analogRead(potpin3);
if (val3 < 100)
{
  s3 = s3 - 2;
  if (s3 <= 10)
  {
    s3 = 10;
  }
  servo3.write(s3);
  delay(5);
}

//Levantar o braço
if (val3 > 900)
{
  s3 = s3 + 2;
  if (s3 >= 170)
  {
    s3 = 170;
  }
  servo3.write(s3);
  delay(5);
}

// Controle da garra do braço
val4 = analogRead(potpin4);
//Abrir a garra
if (val4 < 100)
{
  s4 = s4 - 2;
  if (s4 <= 80)
  {
    s4 = 80;
  }
  servo4.write(s4);
  delay(5);
  ValorVerde = 255;
  ValorVermelho = 20;
  ValorAzul = 150;

}

//Fechar a garra
if (val4 > 900)
{
  s4 = s4 + 2;
  if (s4 >= 130)
  {
    s4 = 130;
  }
  servo4.write(s4);
  delay(5);
}
float colorNumber = counter > numColors ? counter - numColors : counter;

// Play with the saturation and brightness values
// to see what they do
float saturation = 1; // Between 0 and 1 (0 = gray, 1 = full color)
float brightness = 0.5; // Between 0 and 1 (0 = dark, 1 is full brightness)
float hue = (colorNumber / float(numColors)) * 360; // Number between 0 and 360
long color = HSBtoRGB(hue, saturation, brightness);

// Get the red, blue and green parts from generated color
int red = color >> 16 & 255;
int green = color >> 8 & 255;
int blue = color & 255;

setColor(red, green, blue);

// Counter can never be greater then 2 times the number of available colors
// the colorNumber = line above takes care of counting backwards (nicely looping animation)
// when counter is larger then the number of available colors
counter = (counter + 1) % (numColors * 2);

// If you uncomment this line the color changing starts from the
// beginning when it reaches the end (animation only plays forward)
// counter = (counter + 1) % (numColors);

delay(animationDelay);
}

void setColor (unsigned char red, unsigned char green, unsigned char blue)
{
  analogWrite(LED_vermelho, red);
  analogWrite(LED_verde, green);
  analogWrite(LED_azul, blue);
}

long HSBtoRGB(float _hue, float _sat, float _brightness) {
  float red = 0.0;
  float green = 0.0;
  float blue = 0.0;

  if (_sat == 0.0) {
    red = _brightness;
    green = _brightness;
    blue = _brightness;
  } else {
    if (_hue == 360.0) {
      _hue = 0;
    }

    int slice = _hue / 60.0;
    float hue_frac = (_hue / 60.0) - slice;

    float aa = _brightness * (1.0 - _sat);
    float bb = _brightness * (1.0 - _sat * hue_frac);
    float c = _brightness * (1.0 - _sat * (1.0 - hue_frac));

    switch (slice) {
      case 0:
        red = _brightness;
        green = cc;
        blue = aa;
        break;
      case 1:
        red = bb;
        green = _brightness;
        blue = aa;
        break;
      case 2:
        red = aa;
        green = _brightness;
        blue = cc;
        break;
      case 3:
        red = aa;
        green = bb;
        blue = _brightness;
        break;
      case 4:
        red = cc;
        green = aa;
        blue = _brightness;
        break;
      case 5:
        red = _brightness;
        green = aa;
        blue = bb;
        break;
      default:
        red = 0.0;
        green = 0.0;
        blue = 0.0;
        break;
    }
  }

  long ired = red * 255.0;
  long igreen = green * 255.0;
  long iblue = blue * 255.0;

  return long((ired << 16) | (igreen <<  | (iblue));


  Serial.print(val1);
  Serial.print(" : ");
  Serial.print(val2);
  Serial.print(" : ");
  Serial.print(val3);
  Serial.print(" : ");
  Serial.print(val4);
  Serial.println();
}