UNI
UNIVERSIDAD NACIONAL DE INGENIERÍA
FACULTAD DE INGENIERÍA MECÁNICA
4to Laboratorio
TRABAJO 4 LABORATORIO
Integrantes:
Centeno León, Santiago Cristhian García Chávez, Gabriel Enrique Gabino García, Alín Eduardo Salazar Arévalo, Renzo Sebastián
20134006H 20154004K 20091023C 20144046B
SECCIÓN: A
DOCENTE: ING. CALLE FLORES, IVÁN ARTURO FECHA: 07/12/17
2017 – II
UNIVERSIDAD NACIONAL DE INGENIERÍA CÓDIGO DE ARDUINO USADO PARA EL CONTROL DEL MOTOR 1 int IN3 = 5; // Input3 conectada al pin 5 int IN4 = 4; // Input4 conectada al pin 4 int ENB = 3; // ENB conectada al pin 3 de Arduino int incomingByte = 0; float th = -45*3.1416/180;//3.1416; // rad/s max 3.67 float setp = th*103.5737445051; //rad/s float se; int sen; float sensorValue; float posVal; int enc1=6; int a=millis(); float T=20; float e; float V; int PWMV; float kp=0.2;//20//0.1 float ki=0.0;//30 float inte=0; float kd=0;//5 void setup() { Serial.begin(9600); sensorValue = analogRead(A0); attachInterrupt(0, ServicioBoton, FALLING); pinMode(ENB, OUTPUT); pinMode (IN3, OUTPUT); pinMode (IN4, OUTPUT); pinMode (enc1, INPUT); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); sen=1; a=millis(); e=sensorValue-setp; inte=inte+e*T/1000.0; se=max(min(255,-kp*e-ki*inte),0); } void loop() { //if (Serial.available() > 0) { // incomingByte = Serial.read(); // setp=incomingByte*45.0*3.1416/180.0*103.5737445051; // Serial.println("Received: \n"); //} if(millis()>a+T-1){ posVal=posVal+sen*sensorValue*T/1000.0; e=posVal-setp; inte=inte+e*T/1000.0; V=max(-10,min(-kp*e-ki*inte-kd*sen*sensorValue,10)); if (V>0){ digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);} else{ digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);}
Análisis y Control de Robots
2
UNIVERSIDAD NACIONAL DE INGENIERÍA 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(150,PWMV),0); analogWrite(ENB,se);a=millis(); sensorValue = analogRead(A0); //Serial.print(se);Serial.print(',');Serial.print(posVal);Serial.print(','); Serial.print(sensorValue);Serial.print(',');Serial.println(posVal); } }
void ServicioBoton() { sen=(2.0*digitalRead(enc1)-1.0); }
CÓDIGO DE ARDUINO USADO PARA EL CONTROL DEL MOTOR 2
//17 float contador = 0; long a=0; int enc1=3; int IN3 = 7; // Input3 conectada al pin 5 int IN4 = 6; // Input4 conectada al pin 4 int ENB = 5; float n = contador ; int incomingByte = 0; float setp=incomingByte*144/360.0; float v; float T=15; float kp=0.1;//20//0.1 float ki=0.005;//30 float inte=0; float kd=0;//5 float vel=0;//5 float posVal=0;//5 float V; float PWMV; float se; float e; void setup() { Serial.begin(9600); attachInterrupt(0, ServicioBoton, FALLING); pinMode(ENB, OUTPUT); pinMode (enc1, INPUT); pinMode (IN3, OUTPUT); pinMode (IN4, OUTPUT); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); } void loop() { if (Serial.available() > 0) { incomingByte = Serial.read(); setp=incomingByte*45*144/360.0; } if(millis()>a+T-1){
Análisis y Control de Robots
3
UNIVERSIDAD NACIONAL DE INGENIERÍA noInterrupts(); vel=(contador-posVal)/T*1000; posVal=contador; e=posVal-setp; inte=inte+e*T/1000.0; V=max(min(-kp*e-ki*inte,12),-12); if (V>0){ digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); } else{ digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); } PWMV=sqrt(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(150,100*PWMV),0); analogWrite(ENB,se); a=millis(); Serial.print(digitalRead(enc1));Serial.print(',');Serial.println(se); interrupts(); } //v=(360.0/144.0)/(millis()-a)*1000; } void ServicioBoton() { contador=contador+(2.0*digitalRead(enc1)-1.0); }
Análisis y Control de Robots
4
