![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
10/25/2015
Dependencies: PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot
Diff: main.cpp
- Revision:
- 11:9df7ada37d31
- Parent:
- 9:86701fec3f79
- Child:
- 12:8a06a803e373
diff -r 4bc59f8bfa18 -r 9df7ada37d31 main.cpp --- 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