Betago
1
Revision 0:c52fc2fdd2e0, committed 2016-06-27
- Comitter:
- palmdotax
- Date:
- Mon Jun 27 21:14:19 2016 +0000
- Commit message:
- 1
Changed in this revision
diff -r 000000000000 -r c52fc2fdd2e0 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Mon Jun 27 21:14:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
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);*/
+ }
+}
diff -r 000000000000 -r c52fc2fdd2e0 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jun 27 21:14:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file
diff -r 000000000000 -r c52fc2fdd2e0 move.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/move.cpp Mon Jun 27 21:14:19 2016 +0000
@@ -0,0 +1,59 @@
+#include "mbed.h"
+#include "move.h"
+#include "mbed.h"
+DigitalOut dir1(PA_10);//PA_10
+ DigitalOut dir2(PB_3);//PB_3
+ PwmOut speeds(PB_4);
+DigitalOut dirr1(PA_9);//PA_8
+DigitalOut dirr2(PC_7);//PB_5
+PwmOut speeds2(PB_10);
+DigitalOut relays(PA_8);
+
+
+
+ void move:: movespeed_1(float setpoint,float spd)
+{
+ float dc=0;
+ if(setpoint>=0)
+ {
+ dir1=1;
+ dir2=0;
+ // printf("if1\n");
+ }
+ else
+ {
+ dir1=0;
+ dir2=1;
+ // printf("else1\n");
+ }
+ dc=setpoint+spd;
+ // printf("%f\n",dc);
+ speeds2.write(dc);
+
+
+}
+void move:: movespeed_2(float setpoint,float spd)
+{
+ double dc=0;
+ if(setpoint>=0)
+ {
+ dirr1=1;
+ dirr2=0;
+ // printf("if2\n");
+ }
+ else
+ {
+ dirr1=0;
+ dirr2=1;
+ // printf("else2\n");
+ }
+ dc=setpoint+spd;
+ // printf("%f\n",dc);
+ speeds.write(dc);
+
+
+}
+void move::pump(int on_off)
+{
+ relays=on_off;
+}
\ No newline at end of file
diff -r 000000000000 -r c52fc2fdd2e0 move.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/move.h Mon Jun 27 21:14:19 2016 +0000
@@ -0,0 +1,13 @@
+#ifndef MOVE_H
+#define MOVE_H
+#include "mbed.h"
+
+class move
+{
+ public: void movespeed_1(float setpoint,float spd);
+ void movespeed_2(float setpoint,float spd);
+ void pump(int on_off);
+
+
+};
+#endif
\ No newline at end of file