Diff: electrocoagulador_final.cpp

Revision:

7:52b459308d55

Parent:

6:0d2b3f985e41

Child:

8:3649a359ee91

--- a/electrocoagulador_final.cpp	Mon Mar 04 20:40:42 2019 +0000
+++ b/electrocoagulador_final.cpp	Tue Mar 05 13:33:10 2019 +0000
@@ -2,25 +2,29 @@
 #include "keypad.h"
 #include "TextLCD.h"
 #include "stdlib.h"
+#include "QEI.h"
 
 /******************* IO ********************/
 DigitalOut E_1(D8,0);         // Control de electrovalvula 1
 DigitalOut E_2(D9,0);         // Control de electrovalvula 2
 DigitalOut E_3(D10,0);         // Control de electrovalvula 3
 DigitalOut E_4(D12,0);         // Control de electrovalvula 4
-PwmOut     motor(PA_5);         // Variable temporal para el motor (se cambiará después)
+QEI encoder(PD_6,PD_5,NC,4576,QEI::X2_ENCODING);
+PwmOut motor_plus(PB_8);
+PwmOut motor_cero(PB_9);        // Variables para el motor
+
 
 /******************* INTERRUPCIONES ********************/
 Timeout t;
 InterruptIn button(USER_BUTTON);    // Cancelar operación
 
 /*******************VARIABLES********************/
-int tempUser1 = 1;                  // Tiempo tope de electrocoagulacion
-int tempUser2 = 1;                  // Tiempo tope de reposo de la vinaza
-int tempUser3 = 2;                  // Tiempo tope de vaciado del agua en el tanque 2
-int tempUser4 = 1;                  // Tiempo tope de vaciado de residuos en el tanque 2
-int pwm = 50;                       // Duty cycle del motor
-int tiempoLlenado = 1;              // Tiempo de llenado del tanque 1
+int tempUser1 = 5;                  // Tiempo tope de electrocoagulacion
+int tempUser2 = 5;                  // Tiempo tope de reposo de la vinaza
+int tempUser3 = 5;                  // Tiempo tope de vaciado del agua en el tanque 2
+int tempUser4 = 5;                  // Tiempo tope de vaciado de residuos en el tanque 2
+int pwm = 50.0f;                       // Duty cycle del motor
+int tiempoLlenado = 5;              // Tiempo de llenado del tanque 1
 
 int a = 0;
 int b = 0;
@@ -30,6 +34,12 @@
 int canceled = 0;
 int campo = 1;
 
+float y; //vel
+float pwm_pas=50;   
+float e=0;
+float r=1.0f;
+float e_pas=0;
+
 /******************** TECLADO Y LCD ***********************/
 Serial pc(USBTX, USBRX); //rs, e,d4,d5,d6,d7
 TextLCD lcd(PC_8,PC_9,PC_10,PC_11,PC_12,PD_2);
@@ -37,73 +47,96 @@
 
 /********************* FUNCIONES **************************/
 
-void printLCD(){
-    while(1){
+void printLCD()
+{
+    while(1) {
         lcd.cls();
         lcd.locate(0,0);
-            lcd.printf("t1=%d",tempUser1);
+        lcd.printf("t1=%d",tempUser1);
         lcd.locate(6,0);
-            lcd.printf("t2=%d",tempUser2);
+        lcd.printf("t2=%d",tempUser2);
         lcd.locate(0,1);
-            lcd.printf("t3=%d",tempUser3);
+        lcd.printf("t3=%d",tempUser3);
         lcd.locate(6,1);
-            lcd.printf("t4=%d",tempUser4);
+        lcd.printf("t4=%d",tempUser4);
         lcd.locate(12,0);
-            lcd.printf("PWM");
+        lcd.printf("PWM");
         lcd.locate(12,1);
-            lcd.printf("%d",pwm);
-        wait_ms(350);
-        switch(campo){
+        lcd.printf("%d",pwm);
+        wait_ms(150);
+        switch(campo) {
             case  1:
                 lcd.locate(0,0);
-                lcd.printf("t1=   ",tempUser1); break;
+                lcd.printf("t1=   ",tempUser1);
+                break;
             case  2:
                 lcd.locate(6,0);
-                lcd.printf("t2=   ",tempUser2); break;
+                lcd.printf("t2=   ",tempUser2);
+                break;
             case  3:
                 lcd.locate(0,1);
-                lcd.printf("t3=   ",tempUser3); break;
+                lcd.printf("t3=   ",tempUser3);
+                break;
             case  4:
                 lcd.locate(6,1);
-                lcd.printf("t4=   ",tempUser4); break;       
+                lcd.printf("t4=   ",tempUser4);
+                break;
             case  5:
                 lcd.locate(12,1);
                 lcd.printf("   ");
-            default: ; break;
+            default:
+                ;
+                break;
         }
-        wait_ms(350);    
-    }   
+        wait_ms(150);
+    }
 }
 
-int getNum(){
-    while(1){
+int getNum()
+{
+    while(1) {
         char key = keypad.getKey();
         switch(key) {
-            case '0': return 0;
-            case '1': return 1;
-            case '2': return 2;
-            case '3': return 3;
-            case '4': return 4;
-            case '5': return 5;
-            case '6': return 6;
-            case '7': return 7;
-            case '8': return 8;
-            case '9': return 9;
-            case 'A': 
+            case '0':
+                return 0;
+            case '1':
+                return 1;
+            case '2':
+                return 2;
+            case '3':
+                return 3;
+            case '4':
+                return 4;
+            case '5':
+                return 5;
+            case '6':
+                return 6;
+            case '7':
+                return 7;
+            case '8':
+                return 8;
+            case '9':
+                return 9;
+            case 'A':
                 campo++;
-                if(campo==6) campo=1; return -1;
+                if(campo==6) campo=1;
+                return -1;
             //case 'B': return -2;
             //case 'C': return -3;
-            case 'D': return -4;
-            default: ; break;
+            case 'D':
+                return -4;
+            default:
+                ;
+                break;
         }
         wait_ms(100);
     }
-}            
+}
 
 /*********************** MAIN ***************************/
 
-void init(){ 
+void init()
+{
     E_1 = 0;
     E_2 = 0;
     E_3 = 0;
@@ -115,109 +148,145 @@
     lcd.locate(0,1);
     lcd.printf("electrocoagular\n");
     wait(1.5);
-    motor.period(4);
+    
     return;
 }
 
-void setUserTimes(){
-    while(1) {                   
-        setvalue:
+void setUserTimes()
+{
+    while(1) {
+setvalue:
         a = 100*getNum();
-            wait_ms(150);
-            if(a==-1)goto setvalue;
-            if(a==-4)break;
+        wait_ms(200);
+        if(a==-1)goto setvalue;
+        if(a==-4)break;
         b = 10*getNum();
-            wait_ms(150);
-            if(b==-1)goto setvalue;
-            if(b==-4)break;
-        c = getNum(); 
-            wait_ms(150);
-            if(c==-1)goto setvalue;
-            if(c==-4)break;
-        
-        switch(campo){
-        case 1:
-            tempUser1=a+b+c; break;
-        case 2:
-            tempUser2=a+b+c; break;
-        case 3:
-            tempUser3=a+b+c; break;
-        case 4:
-            tempUser4=a+b+c; break;
-        case 5:
-            pwm=a+b+c;         break;
-        default: ;             break;
+        wait_ms(200);
+        if(b==-1)goto setvalue;
+        if(b==-4)break;
+        c = getNum();
+        wait_ms(200);
+        if(c==-1)goto setvalue;
+        if(c==-4)break;
+
+        switch(campo) {
+            case 1:
+                tempUser1=a+b+c;
+                break;
+            case 2:
+                tempUser2=a+b+c;
+                break;
+            case 3:
+                tempUser3=a+b+c;
+                break;
+            case 4:
+                tempUser4=a+b+c;
+                break;
+            case 5:
+                pwm=a+b+c;
+                break;
+            default:
+                ;
+                break;
         }
-        wait_ms(150);
+        wait_ms(200);
     }
-    return;  
+    return;
 }
 
-void vaciadoSolido(){
+void vaciadoSolido()
+{
     E_4 = 0;
-    finished = 1; 
+    finished = 1;
 }
 
-void vaciadoLiquido(){
+void vaciadoLiquido()
+{
     E_3 = 0;
     E_4 = 1;
     t.attach(&vaciadoSolido, tempUser4);
 }
 
-void reposo(){
+void reposo()
+{
     E_3 = 1;
     t.attach(&vaciadoLiquido, tempUser3);
 }
 
-void vaciado1(){
-    E_2 = 0;     
+void vaciado1()
+{
+    E_2 = 0;
     t.attach(&reposo, tempUser2);
+    motor_plus.write(0.0f);
 }
 
-void electro(){
-    motor.write(0.0f);
+void electro()
+{
+    motor_plus.write((float)pwm/100);
+    motor_cero.write(0.0f);
     E_2 = 1;
     t.attach(&vaciado1, tiempoLlenado);
 }
 
-void tiempoValvula1(){
+void tiempoValvula1()
+{
     E_1 = 0;
-    motor.write((float) pwm/100);     
+    //motor_plus.write( pwm/100);
     t.attach(&electro, tempUser1);
+    
+    
+            y = encoder.getSumangle();
+            e=r-y;
+            pwm=(1.639*e)-(1.341*e_pas)+(pwm_pas);
+            pwm_pas=pwm;
+            e_pas=e;
+            motor_plus.write((float)pwm*0.01f);
+    
+    
+    
 }
 
-void cancel_op(){   
+void cancel_op()
+{
     canceled = 1;
     t.detach();
 }
 
-int main(){
-    button.rise(&cancel_op);  
-    while(1){
-        if(running==0){
+int main()
+{
+    
+    motor_plus.period(0.015f);
+    motor_cero.write(0.0f);
+
+    button.rise(&cancel_op);
+    while(1) {
+        if(running==0) {
             running = 1;
             init();
             Thread lcdprint;
             lcdprint.start(printLCD);
             
+
+
             campo = 1;
             setUserTimes();
-            
+
             campo = 0;
             finished = 0;
             canceled = 0;
             E_1 = 1;
-    
-            t.attach(&tiempoValvula1, tiempoLlenado); 
+
+            t.attach(&tiempoValvula1, tiempoLlenado);
+
         }
-        if(finished == 1){
+        if(finished == 1) {
             lcd.cls();
             lcd.locate(0,0);
             lcd.printf("Finalizado\n");
             running = 0;
             wait(2);
         }
-        if(canceled == 1){
+        if(canceled == 1) {
             lcd.cls();
             lcd.locate(0,0);
             lcd.printf("Operacion\n");