Diff: main.cpp

Revision:

0:c52fc2fdd2e0

diff -r 000000000000 -r c52fc2fdd2e0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 27 21:14:19 2016 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "move.h"
+#include "PID.h"
+Serial PC(SERIAL_TX,SERIAL_RX);
+move m1;
+DigitalOut myled(LED1);
+//encoder
+
+InterruptIn encoderA_1(PC_10);//PB_1
+DigitalIn encoderB_1(PC_12);//PB_2
+InterruptIn encoderA_2(PA_5);//PB_14
+DigitalIn encoderB_2(PA_6);//PB_15
+int Encoderpos=0;
+float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125;
+Timer timerStart;
+//timer
+ int timer_now=0,timer_later=0;
+ int times=0,timer_buffer=0;
+ //pid
+
+double setp1=0,setp2=0;
+float outPID =0;
+float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ;
+PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
+PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
+ void EncoderA_1()//ซ้าย
+{   if(encoderB_1==0)
+        { Encoderpos = Encoderpos + 1;}
+    else
+   { Encoderpos = Encoderpos -1;}
+   pulse_1+=1;
+  // PC.printf("m1=%d\n",Encoderpos);
+   
+}
+  void EncoderA_2()//ขวา
+{ 
+    if(encoderB_2==0)
+    { Encoderpos = Encoderpos + 1;}
+    else
+    { Encoderpos = Encoderpos -1;}
+    pulse_2+=1;
+   // PC.printf("m2=%d\n",Encoderpos);
+}
+void getvelo1()// encoder
+{
+    valocity1=pulse_1*((2*3.14*r)/128);
+    PC.printf("valocity1=%f  \n",valocity1);
+    timerStart.reset();
+}
+void getvelo2()
+{
+    valocity2=pulse_2*((2*3.14*r)/128);
+    PC.printf("valocity2=%f  \n",valocity2);
+    timerStart.reset();
+}
+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;
+    
+}
+void PID_m1()//left
+{
+    setp1=map(1.0,0.0,1.094,0.0,1.0);
+    P1.setSetPoint(setp1);
+     times=timerStart.read();
+       if(times==1)// m/s
+       {   
+           getvelo1();
+           //pc.printf("TIME \n");
+           times=0;
+           pulse_1=0;
+        }
+    P1.setProcessValue(valocity1);
+    outPID=P1.compute();
+     //pc.printf("outPID=%f \n",outPID);
+     m1.movespeed_1(setp1,outPID);
+}
+void PID_m2()//right
+{
+    setp2=map(1.0,0.0,1.094,0.0,1.0);
+    P2.setSetPoint(setp2);
+     times=timerStart.read();
+       if(times==1)// m/s
+       {   
+           getvelo2();
+           //pc.printf("TIME \n");
+           times=0;
+           pulse_2=0;
+        }
+    P2.setProcessValue(valocity2);
+    outPID=P2.compute();
+     //pc.printf("outPID=%f \n",outPID);
+     m1.movespeed_2(setp2,outPID);
+}
+int main() {
+    encoderA_1.rise(&EncoderA_1);
+    encoderA_2.rise(&EncoderA_2);
+    PC.baud(115200);
+    
+    
+    while(1) {
+        myled = !myled;
+        m1.movespeed_1(1,0);
+        m1.movespeed_2(1,0);
+        times=timerStart.read();
+       if(times==1)// m/s
+       {   
+           getvelo1();
+           getvelo2();
+           //pc.printf("TIME \n");
+           times=0;
+           pulse_1=0;
+           pulse_2=0;
+        }
+        /*dirr1=0;
+        dirr2=1;
+        speeds2=1;
+        wait(1);*/
+    }
+}