INFORME 4

CODIGO ARDUINO – MOTOR 1

// Variables de Matlab
int setp1,setp2; //Matriz de posiciones
int signo1=0,signo2=0;//Signos de la posicion

//Variables lectura encoder motor

float posMotor; //Grados
int encoder1 = 3; //Pin Amarillo
int encoder2 = 4; //Pin Blanco
float enc;
int a=millis();
int T=25;
float V;
int PWMV;
long int cont = 0; //contador
long int pulse = 0;
float var=0; //#pulsos
int w; //velocidad angular

float angle; //angulo recorrido entre 2 muestras

float ppr =16; //pulsos por revolucion
float mg = 270; // relación de transmisión

//Variables control motor

int IN3 = 9; // Input3 conectada al pin 9

int IN4 = 10; // Input4 conectada al pin 10
int ENB = 11; // ENB conectada al pin 11 de Arduino
float se;
float e=0;
float de=0;
float ie=0;
float kp = 2.0;
float kd = 0.1;
float ki = 0.005;
int i=0;

void setup()
{
Serial.begin(9600);
pinMode (encoder1, INPUT_PULLUP);
pinMode (encoder2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoder1), ServicioBoton, FALLING);
pinMode(ENB, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN4, OUTPUT);
a=millis();

}
void loop()
{

if(Serial.available() > 0)
{
i+=1;
if (i==6){i=1;}
if (i==1){setp1=Serial.read();}
if (i==2){signo1=Serial.read();}
if (i==3){setp2=Serial.read();}
if (i==4){signo2=Serial.read();}
}

if(i==4)
{
Serial.write('a');
delay(100);
Serial.write(setp2);
delay(100);
Serial.write(signo2);
if(signo1==1){setp1=2*setp1;}
else{setp1=-2*setp1;}
if(signo2==1){setp2=2*setp2;}
else{setp2=-2*setp2;}

i+=1;

a=millis();
}

while (i==5)
{
if(millis()>a+T){
var = cont-pulse;
pulse = cont;
angle = (var*enc)*360/(ppr*mg); // POLOLU 64 CPR-CAJA (270:1);

posMotor = posMotor + angle ;

w = (angle)*(1000/T); //grados por segundo

if(posMotor>370){posMotor=0;}
else if(posMotor<-370){posMotor=0;}

de = ((setp1 - posMotor)-e)*1000/T; //ACCION DERIVATIVA

e = setp1 - posMotor; //ACCION PROPORCIONAL
ie=ie+(e*T/1000.0); //ACCION INTEGRAL

V = max(-10,min(-kp*e-kd*de-ki*ie,10));
if (V>0){
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);}
else{
 digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);}

V=abs(V);
PWMV=0.0062*V*V*V*V*V*V - 0.1744*V*V*V*V*V + 1.9355*V*V*V*V -
10.307*V*V*V + 25.965*V*V - 15.652*V + 26.798;
se=max(min(120,PWMV),0);
analogWrite(ENB,se);
Serial.write(w);
a=millis();
}
}
}

void ServicioBoton()
{
enc=2*(digitalRead(encoder2))-1.0; //1->anti, -1->horar
cont+=1;
}

CODIGO ARDUINO MOTOR 2

//Variables del arduino1
int setp2;
int signo2=0;
int w=0;
char flag=0,flag1=0;

// VARIABLES LECTURA ENCODER MOTOR

float posMotor; //Grados
int encoder1 = 3; //Pin Amarillo
int encoder2 = 4; //Pin Blanco
float enc;
int a=millis();
int T=40;
float V;
int PWMV;
long int cont = 0; //contador
long int pulse = 0 ;
float var=0; //#pulsos
float w1;
float w2;

float angle;
float ppr = 16;
float mg = 270;

//VARIABLES CONTROL DEL MOTOR

int IN3 = 9; // Input3 conectada al pin 9
int IN4 = 10; // Input4 conectada al pin 10
int ENB = 11; // ENB conectada al pin 11 de Arduino
float se;
float e=0;
float de=0;
float ie=0;
float kp = 2.0;
float kd = 0.1;
float ki = 0.005;
int i=0;

void setup()
{
Serial.begin(9600);
pinMode (encoder1, INPUT_PULLUP);
pinMode (encoder2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoder1), ServicioBoton, FALLING);
pinMode(ENB, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN4, OUTPUT);
a=millis();
}

void loop()
{

if (Serial.available())
{
if (i==4){i=0;}
if (i==2){signo2=Serial.read();i+=1;}
if (i==1){setp2=Serial.read();i+=1;}
if (i==0){flag = Serial.read();
if (flag=='a'){i+=1;}
}
}

if(i==3)
{
if(signo2==1){setp2=2*setp2;}
else{setp2=-2*setp2;}
i+=1;

a=millis();
}

while(i==4)
{
if(millis()>a+T){

if(Serial.available())
{
flag1 = Serial.read();
if (flag1=='a'){i=1;}
w = (float)flag1;
 }

var = cont-pulse;
pulse = cont;
angle = (var*enc)*360/(ppr*mg); //3 PPR , CAJA DE 270

posMotor = posMotor + angle ;

w2 = (angle*(3.1416/360)*1000)/T;; //radianes por segundo

w1 = w*(3.1416/360); //radianes por segundo
if(posMotor>370){posMotor=0;}
else if(posMotor<-370){posMotor=0;}

de = ((setp2 - posMotor)-e)*1000/T; //ACCION DERIVATIVA

e = setp2 - posMotor; //ACCION PROPORCIONAL
ie=ie+(e*T/1000.0); //ACCION INTEGRAL

V = max(-10,min(-kp*e-kd*de-ki*ie,10));
if (V>0){
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);}
else{
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);}
V=abs(V);

PWMV=0.0062*V*V*V*V*V*V - 0.1744*V*V*V*V*V + 1.9355*V*V*V*V -

10.307*V*V*V + 25.965*V*V - 15.652*V + 26.798;
se=max(min(120,PWMV),0);
analogWrite(ENB,se);

Serial.print(posMotor);Serial.print(';');Serial.println(w2);
a=millis();
}
}
}

void ServicioBoton()
{
enc=2*(digitalRead(encoder2))-1.0; //1->anti, -1->horar
cont+=1;
}

CODIGO COMUNICACIÓN DE MATLAB

//Variables del arduino1
int setp2;
int signo2=0;
int w=0;
char flag=0,flag1=0;

// VARIABLES LECTURA ENCODER MOTOR

float posMotor; //Grados
int encoder1 = 3; //Pin Amarillo
int encoder2 = 4; //Pin Blanco
float enc;
int a=millis();
int T=40;
float V;
int PWMV;
long int cont = 0; //contador
long int pulse = 0 ;
float var=0; //#pulsos
float w1;
float w2;

float angle;
float ppr = 16;
float mg = 270;

//VARIABLES CONTROL DEL MOTOR

int IN3 = 9; // Input3 conectada al pin 9
int IN4 = 10; // Input4 conectada al pin 10
int ENB = 11; // ENB conectada al pin 11 de Arduino
float se;
float e=0;
float de=0;
float ie=0;
float kp = 2.0;
float kd = 0.1;
float ki = 0.005;
int i=0;

void setup()
{
Serial.begin(9600);
pinMode (encoder1, INPUT_PULLUP);
pinMode (encoder2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoder1), ServicioBoton, FALLING);
pinMode(ENB, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN4, OUTPUT);
a=millis();
}

void loop()
{

if (Serial.available())
{
if (i==4){i=0;}
if (i==2){signo2=Serial.read();i+=1;}
if (i==1){setp2=Serial.read();i+=1;}
 if (i==0){flag = Serial.read();
if (flag=='a'){i+=1;}
}
}

if(i==3)
{
if(signo2==1){setp2=2*setp2;}
else{setp2=-2*setp2;}
i+=1;

a=millis();
}

while(i==4)
{
if(millis()>a+T){

if(Serial.available())
{
flag1 = Serial.read();
if (flag1=='a'){i=1;}
w = (float)flag1;
}

var = cont-pulse;
pulse = cont;
angle = (var*enc)*360/(ppr*mg); //3 PPR , CAJA DE 270

posMotor = posMotor + angle ;

w2 = (angle*(3.1416/360)*1000)/T;; //radianes por segundo

w1 = w*(3.1416/360); //radianes por segundo
if(posMotor>370){posMotor=0;}
else if(posMotor<-370){posMotor=0;}

de = ((setp2 - posMotor)-e)*1000/T; //ACCION DERIVATIVA

e = setp2 - posMotor; //ACCION PROPORCIONAL
ie=ie+(e*T/1000.0); //ACCION INTEGRAL

V = max(-10,min(-kp*e-kd*de-ki*ie,10));
if (V>0){
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);}
else{
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);}
V=abs(V);

PWMV=0.0062*V*V*V*V*V*V - 0.1744*V*V*V*V*V + 1.9355*V*V*V*V -

10.307*V*V*V + 25.965*V*V - 15.652*V + 26.798;
se=max(min(120,PWMV),0);
 analogWrite(ENB,se);

Serial.print(posMotor);Serial.print(';');Serial.println(w2);
a=millis();
}
}
}

void ServicioBoton()
{
enc=2*(digitalRead(encoder2))-1.0; //1->anti, -1->horar
cont+=1;
}