Cursos

Ferramentas do usuário

Ferramentas do site

cursos:introrobotica:2019-1:grupo04:funcoes_tp4.h
#ifndef FUNCOES_H
#define FUNCOES_H
 
#include <Adafruit_MotorShield.h>
#include "controladores.h"
#include "sensores.h"
#define RaioRoda 0.04
#define RaioRobo 0.07
#include <LiquidCrystal.h>
 
 
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
 
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);//motor lego
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
 
int v1 = 0; // usado para aumentar a velocidade do motor 1
int v2 = 0; // usado para aumentar a velocidade do motor 2
//int tempo_carro = 10;
float t_gir180 = 2.1;
float t_dir90 = 1.025;
float t_esq90 = 0.8;
float t_dir45 = 1.5;
//float t90 = 1.40;
float t_frente = 2.3;
float t_re = 2;
 
int vel1 = 120; // velocidade inicial do motor 1 - 140 - 120
int vel2 = 135;// velocidade inicial do motor 2 - 160 - 135
 
void gira_esq (int angulo){
  float velangdes =2.0;
  float velin= velangdes*RaioRoda;
  float t =(((PI*RaioRobo)/180)*angulo)/(velin); 
  float velang;
  float velang2;
  int u=0;
  int u2=0;
  motor1->setSpeed(u);
  motor2->setSpeed(u2);
  long  tempoinicial = millis();
  long  tempoatual = tempoinicial;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  while(tempoatual < tempoinicial + (t*1000)){
    velang = Leitura_Encoder1();
    velang2 = Leitura_Encoder2();
    u = controlador1(velang,velangdes, u);
    u2 = controlador2(velang2,velang,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
//     lcd.setCursor(0,0); 
//        lcd.print("                "); 
//        lcd.setCursor(0,0); 
//        lcd.print(t);
//        lcd.print(" ");
//        lcd.print(" ");
//        lcd.setCursor(0,1); 
//        lcd.print(velang);
//        lcd.print(" ");
//        lcd.print(velang2);
//        lcd.print("   ");
 
     if (t_atual > (t_start +60000)){
      break;
    } 
    t_atual = millis();   
    tempoatual = millis();
  }
  motor1->run(RELEASE);
  motor2->run(RELEASE);
 
  }
 
 
 
 
 
 
void gira_dir (int angulo){
 float velangdes =2.5;
  float velin= velangdes*RaioRoda;
  float t =(((PI*RaioRobo)/180)*angulo)/(velin); 
  float velang;
  float velang2;
  int u=40;
  int u2=120;
  motor1->setSpeed(u);
  motor2->setSpeed(u2);
  long  tempoinicial = millis();
  long  tempoatual = tempoinicial;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  while(tempoatual < tempoinicial + (t*1000)){
    velang = Leitura_Encoder1();
    velang2 = Leitura_Encoder2();
    u = controlador1(velang,velangdes, u);
    u2 = controlador2(velang2,velangdes,u2);
 
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
//     lcd.setCursor(0,0); 
//        lcd.print("                "); 
//        lcd.setCursor(0,0); 
//        lcd.print(t);
//        lcd.print(" ");
//        lcd.print(" ");
//        lcd.setCursor(0,1); 
//        lcd.print(velang);
//        lcd.print(" ");
//        lcd.print(velang2);
//        lcd.print("   ");
     if (t_atual > (t_start +60000)){
      break;
    } 
    t_atual = millis(); 
    tempoatual = millis();
  }
    motor1->run(RELEASE);
  motor2->run(RELEASE);
}
 
//void dir45 (float t){
//  t = t*1000;
//  motor1->run(BACKWARD);
//  motor2->run(FORWARD);
//  delay(t);
//}
 
void para(){
  motor1->run(RELEASE);
  motor2->run(RELEASE);
  delay(500);
}
 
void frente(float d){
    float velangdes = 2.5;
    float velangdes2 = 2.5;
    float velang;
    float velang2;
    float velindes = velangdes * RaioRoda;
    float t = d/velindes;
    int u=0;
    int u2=120;
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
    long  tempoinicial = millis();
    long  tempoatual = tempoinicial;
    unsigned long tempo_inicio;
    unsigned long tempo_fim;
    motor1->run(FORWARD);
    motor2->run(FORWARD);
 
    while(tempoatual<tempoinicial+(t*1000)){
        tempo_inicio = micros();
        velang = Leitura_Encoder1();
        velang2 = Leitura_Encoder2();
 
 
        u = controlador1(velang,velangdes, u);
        u2 = controlador2(velang2,velang,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
           //send-receive with processing if it's time
          if(millis()>serialTime)
          {
            SerialReceive();
            SerialSend();
            serialTime+=500;
          }
 
 
//         lcd.setCursor(0,0); 
//        lcd.print("                "); 
//        lcd.setCursor(0,0); 
//        lcd.print(t);
//        lcd.print(" ");
//        lcd.print(" ");
//        lcd.setCursor(0,1); 
//        lcd.print(velang);
//        lcd.print(" ");
//        lcd.print(velang2);
//        lcd.print("   ");
 velang = 0;
 velang2 = 0; 
 
   if (t_atual > (t_start +60000)){
      break;
    } 
    t_atual = millis();  
 tempoatual = millis(); 
 
 tempo_fim = micros();
 Serial.println(tempo_fim - tempo_inicio);   
}
  motor1->run(RELEASE);
  velang = 0;
  motor2->run(RELEASE);
  velang2 = 0;
}
 
 
void re(float d){
    float velangdes = 2.5;
    float velangdes2 = 2.5;
    float velang;
    float velang2;
    float t = d/velangdes;
    int u=15;
    int u2=90;
    motor1->setSpeed(u);
    motor2->setSpeed(u2);
    long  tempoinicial = millis();
    long  tempoatual = tempoinicial;
    motor1->run(BACKWARD);
    motor2->run(BACKWARD);
    while(tempoatual<tempoinicial+(t*1000)){
        velang = Leitura_Encoder1();
        velang2 = Leitura_Encoder2();
        u = controlador1(velang,velangdes, u);
        u2 = controlador2(velang,velangdes2,u2);
 
        motor1->setSpeed(u);
        motor2->setSpeed(u2);
 
           //send-receive with processing if it's time
          if(millis()>serialTime)
          {
            SerialReceive();
            SerialSend();
            serialTime+=500;
          }
 
 
        lcd.setCursor(0,0); 
        lcd.print("                "); 
        lcd.setCursor(0,0); 
        lcd.print(vel1);
        lcd.print(" ");
        lcd.print(vel2);
        lcd.setCursor(0,1); 
        lcd.print(velang);
        lcd.print(" ");
        lcd.print(velang2);
        lcd.print("   ");
    if (t_atual > (t_start +60000)){
      break;
    } 
    t_atual = millis(); 
    tempoatual = millis();
 
}
  motor1->run(RELEASE);
  motor2->run(RELEASE);
}
 
 
void direita(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(RELEASE);
  delay(t);
}
 
void esquerda(float t){
  t = t*1000;
  motor1->run(RELEASE);
  motor2->run(BACKWARD);
  delay(t);
}
 
// gira horario
void gira(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void giraesquerda(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
//gira antihorario
void giraanti(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
void esquerda(){
  int tempo=0;
 
  para();
  esquerda(20);
  para();
 
}
 
void reta(){
  frente(t_frente);
  para();
  gira(t_gir180);
  para();
  frente(t_frente);
  para();
}
 
void triangulo(){
  frente(t_frente);
  para();
  gira(t_dir45);
  para();
  float t = 2.8;
  frente(t);
  para();
  gira(t_dir45);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
void quadrado(){
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
// Aumenta velocidade motor 2
void AumentaVel2() {
  if (vel2 <= 256){
    vel2 = vel2 + 1;
 
  }
  else{
    vel2 = 100;
 }
} 
 
void AumentaVel1() {
  if (vel1 < 256){
    vel1 = vel1 + 1;
 
  }
  else{
    vel1 = 100;
 }
} 
 
// DIMINUI velocidade motor 2
void DiminuiVel2() {
 
    vel2 = vel2 - 1;
 
 
 
} 
 
void DiminuiVel1() {
  if (vel1 < 210){
    vel1 = vel1 - 1;
  }
  else{
    vel1 = 100;
 }
} 
 
 
void Teste_Encoder()  
{  
  int pulso = 0;
  int pulso2 = 0;
  long transicao = 0;
  long transicao2 = 0;
  int estadoant;
  int estado;
  int estadoant2;
  int estado2;
  long rotacao=0;
  long rotacao2=0;
  long transicaoant = 0;
  long transicaoant2 = 0;
 
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  long tempoinicial = millis();
  long tempotr = tempoinicial;
  long tempoatual = tempoinicial;
  long tempotr2 = tempoinicial;
  long tempoatual2 = tempoinicial;
  long tempotr_ant;
  long tempotr_ant2;
  float velang = 0;
  float vellin = 0;
  float velang2 = 0;
  float vellin2 = 0;
 
  int dif = 0;
  int dif2 = 0;
 
 
  while(1){
 
  estado = analogRead(enc);  
  estado2 = analogRead(enc2);
  //Serial.print (estado);
  //Serial.print (" ");
  //-------------------M1------------------------------------  
  if (estado >= 1000 && estadoant < 1000){
    pulso = 1;
    transicao++;
    tempotr_ant = tempotr;
    tempotr = millis();
    dif = tempotr - tempotr_ant;
    velang = (PI/3)/((dif)*pow(10,-3));
    vellin = velang * RaioRoda;
  }
  else if(estado < 1000 && estadoant >= 1000){
    pulso = 0;
    transicao++;
    //tempotr_ant = tempotr;
    //tempotr = millis();
    //dif = tempotr - tempotr_ant;
    //velang = (PI/6)/((dif)*pow(10,-3));
    //vellin = velang * RaioRoda;
  }
  estadoant = estado;
 
   if (transicao % 12 == 0 && transicaoant == (transicao - 1))
  {rotacao++;
  }
 
  transicaoant = transicao;
  //-------------------M2------------------------------------  
  if (estado2 >= 1000 && estadoant2 < 1000){
    pulso2 = 1;
    transicao2++;
    tempotr_ant2 = tempotr2;
    tempotr2 = millis();
    dif2 = tempotr2 - tempotr_ant2;
    velang2 = (PI/3)/((dif2)*pow(10,-3));
    vellin2 = velang * RaioRoda;
  }
  else if(estado2 < 1000 && estadoant2 >= 1000){
    pulso2 = 0;
    transicao2++;
    //tempotr_ant = tempotr;
    //tempotr = millis();
    //dif = tempotr - tempotr_ant;
    //velang = (PI/6)/((dif)*pow(10,-3));
    //vellin = velang * RaioRoda;
  }
  estadoant2 = estado2;
 
  if (transicao2 % 12 == 0 && transicaoant2 == (transicao2 - 1))
    {rotacao2++;
    }
 
  transicaoant2 = transicao2;
 
  //---------------PRINTS------------------------------------
//  Serial.print("Rotacao: ");
//  Serial.println(rotacao);
//  Serial.print("pulsos: ");
 
//  Serial.print("Transicao: ");
//  Serial.println(transicao);
//  Serial.print("Vel angular: ");
//  Serial.println(velang);
//  Serial.print("Vel linear: ");
//  Serial.println(vellin);
//  Serial.print("tempo transiçao anterior: ");
//  Serial.println(tempotr_ant);
//  Serial.print("tempo transicao atual: ");
//  Serial.println(tempotr);
//  Serial.print("diferença: ");
 
  //Serial.print(dif);
 
  //Serial.print(" ");
 
 
//  Serial.print("veln: ");
//  Serial.println(vel2);
 
 
 
//---------------------------------aumenta e diminui velocidades------------------//  
  int bot;  
  bot = analogRead (0);
 
  if (bot < 100) {  
    // ---------------------- BOTAO RIGHT -----------------
    AumentaVel1();
    motor1->setSpeed(vel1);
    //delay(300);
  } 
  else if (bot < 200) {
    // ---------------------- BOTAO UP -----------------
    AumentaVel2();
    motor2->setSpeed(vel2);
    //localiza();
   // delay(300);
  }  
  else if (bot < 400){ 
    // -------------------- BOTAO DOWN --------------  
    DiminuiVel2() ;
    motor2->setSpeed(vel2);
    //checaCores();    
    //delay (300);
 
  }
    else if (bot < 600){ 
    // -------------------- BOTAO LEFT -------------- 
    //encoder();
    DiminuiVel1() ;
    motor1->setSpeed(vel1);
    //delay(300);
  }
//---------------------------printa LCD------------------------// 
  lcd.setCursor(0,0); 
  lcd.print("                "); 
  lcd.setCursor(0,0); 
  lcd.print(vel1);
  lcd.print(" ");
  lcd.print(vel2);
  lcd.setCursor(0,1); 
  lcd.print("                ");    
  lcd.setCursor(0,1); 
  lcd.print(velang);
  lcd.print(" ");
  lcd.print(velang2);
 
 
 
  tempoatual = millis();
  }
 
 
 
  motor1->run(RELEASE);
  motor2->run(RELEASE);
  }
 
 
  void modelo(){
  float velang1;
  float velang2;
  motor1->setSpeed(100);
  motor2->setSpeed(100);
  motor1->run(FORWARD);
  motor2->run(FORWARD);
 
  long t_at = millis();
  long t_ant = t_at;
  while( t_at < t_ant + 7000){
    velang1 = Leitura_Encoder1(); 
    velang2 = Leitura_Encoder2();
    Serial.print(velang1);
    Serial.print(" ");
    Serial.println(velang2);
 
   t_at = millis();
 
  }
  motor1->setSpeed(200);
  motor2->setSpeed(200);
  Serial.println("-------------------------------------------------------------------------");
  long t_ant2 = millis();
  t_at = millis();
  while( t_at < t_ant2 + 7000){
    velang1 = Leitura_Encoder1(); 
    velang2 = Leitura_Encoder2();
    Serial.print(velang1);
    Serial.print(" ");
    Serial.println(velang2);
 
   t_at = millis();
 
  }
  motor1->run(RELEASE);
  motor2->run(RELEASE);
 
  }
 
 
 
#endif
cursos/introrobotica/2019-1/grupo04/funcoes_tp4.h.txt · Última modificação: por 127.0.0.1
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki