Diff: main.cpp

Revision:

9:86701fec3f79

Parent:

8:8fbc0c858875

Child:

11:9df7ada37d31

--- a/main.cpp	Mon Dec 14 19:35:08 2015 +0000
+++ b/main.cpp	Sun Jan 17 20:34:12 2016 +0000
@@ -3,55 +3,83 @@
 #include "PID.h"
 
 DigitalOut led1(LED1);
-InterruptIn encoderA(D10);
-InterruptIn encoderB(D11);
+InterruptIn encoderA_d(PB_12);
+DigitalIn encoderB_d(PB_13);
+InterruptIn encoderA_1(PB_1);
+DigitalIn encoderB_1(PB_2);
+InterruptIn encoderA_2(PB_14);
+DigitalIn encoderB_2(PB_15);
 Timer timerStart;
 Timeout timecount;
 move m1;
 PID P1(0.005,0.005,0,0.1);
 double setp1=0,setp2=0;
 //DigitalIn encoderB(D5);
-int times;
+
 
-Serial pc(SERIAL_TX,SERIAL_RX);
- int Encoderpos = 0;
- float valocity =0,pulse=0,count=0,r=0.125,velocityreal=0;
+ Serial pc(SERIAL_TX,SERIAL_RX);
+ int Encoderpos = 0,times=0;
+ int real_d=0;
+ float valocity =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
  float outPID =0;
  //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;
 
 
-void EncoderA()
-{   if(encoderB==0)
+void EncoderA_1()
+{   if(encoderB_1==0)
         { Encoderpos = Encoderpos + 1;}
     else
    { Encoderpos = Encoderpos -1;}
-   
+   pulse_1+=1;
    //Encoderpos = Encoderpos + 1;
   // valocity+=1;
   // pc.printf("%d \n",Encoderpos);
-  pulse+=1;
-  //pc.printf("pulse=%f  \n",pulse);
+   //pc.printf("pulse=%f  \n",pulse);
  // if(pulse==128)
   //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
-  
 }
-  
-
-/*void EncoderB()
+  void EncoderA_2()
 { 
-    if(encoderA==1)
+    if(encoderB_2==0)
+    { Encoderpos = Encoderpos + 1;}
+    else
+    { Encoderpos = Encoderpos -1;}
+    pulse_2+=1;
+    //pc.printf("%d",Encoderpos);
+}
+void EncoderA_D()
+{ 
+    if(encoderB_d==0)
     { Encoderpos = Encoderpos + 1;}
     else
     { Encoderpos = Encoderpos -1;}
-    pc.printf("%d",Encoderpos);
-}*/
-void getvelo()
+    pulse_d+=1;
+    if(pulse_d==128)
+    {
+        Z_d+=1;
+        pulse_d=0;
+    }
+   // pc.printf("%d",Encoderpos);
+}
+void getvelo1()
 {
-    valocity=pulse*((2*3.14*r)/128);
+    valocity=pulse_1*((2*3.14*r)/128);
     pc.printf("valocity=%f  \n",valocity);
     count=0;
     timerStart.reset();
 }
+void getvelo2()
+{
+    valocity=pulse_2*((2*3.14*r)/128);
+    pc.printf("valocity=%f  \n",valocity);
+    count=0;
+    timerStart.reset();
+}
+void get_d()
+{
+    real_d=Z_d*(2*3.14*r);
+    
+}
 double map(double x, double in_min, double in_max, double out_min, double out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -64,10 +92,10 @@
      times=timerStart.read();
        if(times==1)// m/s
        {   
-           getvelo();
+           getvelo1();
            //pc.printf("TIME \n");
            times=0;
-           pulse=0;
+           pulse_1=0;
         }
     P1.setProcessValue(valocity);
     outPID=P1.compute();
@@ -81,10 +109,10 @@
      times=timerStart.read();
        if(times==1)// m/s
        {   
-           getvelo();
+           getvelo2();
            //pc.printf("TIME \n");
            times=0;
-           pulse=0;
+           pulse_2=0;
         }
     P1.setProcessValue(valocity);
     outPID=P1.compute();
@@ -94,10 +122,10 @@
 
 int main()
 {
-       int times=0;
+      
        
        pc.baud(115200);
-       encoderA.rise(&EncoderA);
+       encoderA_1.rise(&EncoderA_1);
        timerStart.start();   
        P1.setMode(1);
        //setp=map(1,0,1.0916,0,1);