Diff: main.cpp

Revision:

0:27f6b9086106

diff -r 000000000000 -r 27f6b9086106 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 27 23:41:33 2020 +0000
@@ -0,0 +1,223 @@
+#include "mbed.h"
+#include "PID.h"
+#include "motoresDC.h"
+#include "rtos.h"
+
+InterruptIn PIN_in(D2);
+DigitalOut r1(D3);
+DigitalOut r2(D4);
+MotoresDC carro(PTE31,D3, D4, D9, D12, D13);
+Timer t;
+Serial pc(USBTX, USBRX);
+Thread thread1;
+Thread thread2;
+Thread thread3;
+Thread thread4;
+
+Semaphore one_slot(1);
+Mutex mutex1;
+
+volatile uint8_t n;
+float timeold;
+unsigned int N;
+int x=0,z=0,q=0,dato=-1,dato1,dato2,dato3,dato4,dato5,dato6,dato7,dato8;
+float v,h,dkp,dki,dkd,dset,N1;
+float kp=2.0,ki=5.0,kd=1.0;
+
+PID mypid(kp,ki,kd,0.01);
+void enco()
+{
+    one_slot.release();
+    one_slot.wait();
+    n++;
+    one_slot.release();
+}
+void interrupcion_serial(void)
+{
+    one_slot.release();
+    one_slot.wait();
+    dato=pc.getc();
+    if(z==8) {
+        z=0;
+    }
+    one_slot.release();
+
+}
+void motor()
+{
+    while(1) {
+        if(q==8) {
+
+            if (t.read_ms() - timeold >= 1000) {
+
+                N = ( n * 60 ) / (( t.read_ms() - timeold )/ 1000 * 20 );
+                n = 0;
+                timeold = t.read_ms();
+                //v = N * 3.1416 * 24 * 60 / 1000000;
+                N1=(float(N)*(3.3))/120.0;
+
+                //Thread::wait(1000);
+
+            }
+
+        }
+    }
+}
+void piD (){
+    while(1){
+            one_slot.release();
+            one_slot.wait();
+            mypid.setSetPoint(dset);
+            mypid.setTunings(dkp,dki,dkd);
+            mypid.setProcessValue(float(N));
+            h=mypid.compute();
+            carro.motorIzq(h);
+            one_slot.release();
+        }
+    }
+void serial()
+{
+    while(1) {
+        if(q==8) {
+
+            one_slot.release();
+            one_slot.wait();
+            //pc.printf("kp: %f ki: %f kd: %f set: %f ",dkp,dki,dkd,dset);
+            //pc.printf("RPM: %d km/h: %f ",N,v);
+           // pc.printf("RPM: %d N1: %f ",N,N1);
+            //pc.printf("PWM: %f setPont: %f \n\r",h,dset);
+            int N3=int(N*42);
+            pc.printf("%d\n",N3);
+            Thread::wait(300);
+            one_slot.release();
+
+        }
+    }
+}
+void conver()
+{
+    switch(dato) {
+        case '1':
+            x=1;
+            break;
+        case '2':
+            x=2;
+            break;
+        case '3':
+            x=3;
+            break;
+        case '4':
+
+            x=4;
+            break;
+        case '5':
+            x=5;
+            break;
+        case '6':
+            x=6;
+            break;
+        case '7':
+            x=7;
+            break;
+        case '8':
+            x=8;
+            break;
+        case '9':
+            x=9;
+            break;
+        case '0':
+            x=0;
+            break;
+    }
+
+}
+void inicial()
+{
+    while(1) {
+        if(z==0 && dato>0) {
+            conver();
+            dato1=x;
+            dato=-1;
+            //  pc.printf("d1: %d  \n\r",dato1);
+            z=1;
+        } else if (z==1 && dato>0) {
+            conver();
+            dato2=x;
+            dato=-1;
+            //  pc.printf("d2: %d  \n\r",dato2);
+            z=2;
+            dkp=float(dato1)+float(dato2*0.1);
+            //  pc.printf("dkp: %f  \n\r",dkp);
+        } else if (z==2 && dato>0) {
+            conver();
+            dato3=x;
+            dato=-1;
+            //  pc.printf("d3: %d  \n\r",dato3);
+            z=3;
+        } else if (z==3 && dato>0) {
+            conver();
+            dato4=x;
+            dato=-1;
+            //    pc.printf("d4: %d \n\r",dato4);
+            z=4;
+            dki=float(dato3)+float(dato4*0.1);
+            //    pc.printf("dki: %f  \n\r",dki);
+
+        } else if (z==4 && dato>0) {
+            conver();
+            dato5=x;
+            dato=-1;
+            //  pc.printf("d5: %d  \n\r",dato5);
+            z=5;
+        } else if (z==5 && dato>0) {
+            conver();
+            dato6=x;
+            dato=-1;
+            //     pc.printf("d6: %d \n\r",dato6);
+            z=6;
+            dkd=float(dato5)+float(dato6*0.1);
+            //     pc.printf("dkd: %f  \n\r",dkd);
+
+        } else if (z==6 && dato>0) {
+            conver();
+            dato7=x;
+            dato=-1;
+            //  pc.printf("d7: %d  \n\r",dato7);
+            z=7;
+        } else if (z==7 && dato>0) {
+            conver();
+            dato8=x;
+            dato=-1;
+            //  pc.printf("d8: %d m: %d \n\r",dato8);
+            z=8;
+            q=8;
+            dset=float(dato7*10)+float(dato8);
+            //   pc.printf("dset: %f  \n\r",dset);
+            mypid.reset();
+
+        }
+
+
+    }
+}
+int main()
+{
+    pc.baud(9600);
+    PIN_in.mode(PullUp);
+    PIN_in.fall(&enco);
+    pc.attach(&interrupcion_serial);
+    thread1.start(motor);
+    thread2.start(serial);
+    thread3.start(inicial);
+    thread4.start(piD);
+    mypid.setOutputLimits(0.0,1.0);
+    mypid.setMode(1);
+    carro.frenarIzq();
+    carro.detenerIzq();
+    r1=1;
+    r2=0;
+    t.start();
+    thread4.join();
+
+
+}
\ No newline at end of file