Diff: main.cpp

Revision:

11:9df7ada37d31

Parent:

9:86701fec3f79

Child:

12:8a06a803e373

--- a/main.cpp	Sun Jan 17 21:28:54 2016 +0000
+++ b/main.cpp	Wed Mar 02 23:10:18 2016 +0000
@@ -1,6 +1,7 @@
 #include"mbed.h"
 #include "move.h"
 #include "PID.h"
+#include "UNTRASONIC.h"
 
 DigitalOut led1(LED1);
 InterruptIn encoderA_d(PB_12);
@@ -10,21 +11,29 @@
 InterruptIn encoderA_2(PB_14);
 DigitalIn encoderB_2(PB_15);
 Timer timerStart;
-Timeout timecount;
+Timeout time_getsensor;
+Timeout time_distance;
+Timeout shutdown;
 move m1;
 PID P1(0.005,0.005,0,0.1);
 double setp1=0,setp2=0;
 //DigitalIn encoderB(D5);
-
-
+ 
+ int timer_now=0,timer_later=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;
+// bool timeout_state=0;
  //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;
-
-
+ sensor s1;
+void Rx_interrupt()
+{
+    //s1.get_motor();รับค่ามอเตอร์
+    timer_later= timer_now;
+  
+}
 void EncoderA_1()
 {   if(encoderB_1==0)
         { Encoderpos = Encoderpos + 1;}
@@ -32,11 +41,11 @@
    { Encoderpos = Encoderpos -1;}
    pulse_1+=1;
    //Encoderpos = Encoderpos + 1;
-  // valocity+=1;
-  // pc.printf("%d \n",Encoderpos);
+   //valocity+=1;
+   //pc.printf("%d \n",Encoderpos);
    //pc.printf("pulse=%f  \n",pulse);
- // if(pulse==128)
-  //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
+   //if(pulse==128)
+   //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
 }
   void EncoderA_2()
 { 
@@ -61,7 +70,7 @@
     }
    // pc.printf("%d",Encoderpos);
 }
-void getvelo1()
+void getvelo1()//จาก encoder
 {
     valocity=pulse_1*((2*3.14*r)/128);
     pc.printf("valocity=%f  \n",valocity);
@@ -75,9 +84,10 @@
     count=0;
     timerStart.reset();
 }
-void get_d()
+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)
@@ -119,6 +129,10 @@
      //pc.printf("outPID=%f \n",outPID);
      m1.movespeed_2(1,setp2,outPID);
 }
+void getSensor()
+{
+    s1.get_sen();
+}
 
 int main()
 {
@@ -128,41 +142,23 @@
        encoderA_1.rise(&EncoderA_1);
        timerStart.start();   
        P1.setMode(1);
-       //setp=map(1,0,1.0916,0,1);
-     //  setp=map(1.0,0.0,1.094,0.0,1.0);
-       
-      // P1.setSetPoint(velocityreal);//setpont
-     //   P1.setSetPoint(setp);//setpont
        P1.setBias(0);
-       pc.printf("READY \n");
-      
+     //  pc.printf("READY \n");
+       pc.attach(&Rx_interrupt, Serial::RxIrq);
        led1=1;
     while(1)
     
-    {  
-    
-    
-    /*
-       // pc.printf("%f  \n",setp);
-       times=timerStart.read();
-       if(times==1)// m/s
-       {   
-           getvelo();
-           //pc.printf("TIME \n");
-           times=0;
-           pulse=0;
-        }
-      
-       P1.setProcessValue(valocity);
-       // m1.movespeed(1,1.0);
-        outPID=P1.compute();
-         pc.printf("outPID=%f \n",outPID);
-        m1.movespeed(1,setp,outPID);
-       // m1.movespeed(1,1,0);
-        
-        wait(0.1);
-    */
-       
-         
-    }  
+    { 
+     timer_now=timerStart.read();
+     if((timer_now-timer_later)>2)
+       {
+            m1.movespeed_1(1,0,0);
+            m1.movespeed_2(1,0,0);
+       }
+       //shutdown.attach_us(&Timeout, 2.0);
+       time_getsensor.attach_us(&getSensor, 2.0);
+       time_distance.attach_us(&get_d, 2.0);
+       PID_m1();
+       PID_m2();
+       }  
 }
\ No newline at end of file