譯文 張
/
BX-car
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 11:ffd762ae141b, committed 2014-06-25
- Comitter:
- even
- Date:
- Wed Jun 25 05:59:29 2014 +0000
- Parent:
- 10:d2401a243e8d
- Commit message:
- 1st PID complete
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Jun 25 05:07:13 2014 +0000
+++ b/main.cpp Wed Jun 25 05:59:29 2014 +0000
@@ -50,6 +50,10 @@
double motor;
double b_r_c;
double PID_v;
+
+ double Kp = 0.0005, Ki = 0.0001, Kd = 0.0001;
+ double integral = 0, derivative = 0, error, last_error = 0, turn;
+
while(1){
#ifdef task_ma_time
@@ -145,16 +149,21 @@
//PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
//pc.printf("%f %d %d speed :%f bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
pc.printf("b_r_c %lf\r\n",b_r_c);
- int error = b_r_c-64;
- if(error<8&&error>-8){
+
+ error = b_r_c - 64;
+ integral = 0.3 * integral + error; //0.3 is the factor to make old datas become smaller
+ derivative = error - last_error;
+
+ turn = error*Kp + integral*Ki + derivative*Kd;
+
+ if(error < 8 && error > -8){
servo.set_angle(0.085);
}
- else if(error<-8){
- servo.set_angle(0.1);
+ else{
+ servo.set_angle(0.085 + turn);
}
- else if(error>8){
- servo.set_angle(0.07);
- }
+
+ leas_error = error;
//servo.set_angle(PID_v);