Clark Lin
QQQ
Diff: main.cpp
- Revision:
- 10:9f0ce6ba7663
- Parent:
- 9:33b99cb45e99
diff -r 33b99cb45e99 -r 9f0ce6ba7663 main.cpp
--- a/main.cpp Tue Jun 24 10:06:54 2014 +0000
+++ b/main.cpp Thu Jun 26 14:29:53 2014 +0000
@@ -11,156 +11,153 @@
#define motor_on
#define Pcontroller
#define task_ma_time
+#define offset 65
+#define kp 1
+#define ki 0.1
+#define kd 1
#include "TSISensor.h"
Serial pc(USBTX, USBRX);
-
-
BX_servo servo;
-
BX_camera cam;
-
BX_motor MotorA('A');
BX_motor MotorB('B');
-
-BX_pot pot1('1');
-
- // 90/30=3
+BX_pot pot1('1'); // 90/30=3
+BX_pot pot2('2');
PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
-
DigitalOut task_pin(PTD1);
TSISensor tsi;
-int main() {
+
+double Computes(double, double, double);
+void run();
+
+int main(){
+ pc.baud(115200);
+ while(1){
+ if(tsi.readPercentage()>0.00011)
+ break;
+ }
+ run();
- pc.baud(115200);
+ return 0;
+ }
+
+double Computes(double b_r_c, double* last_I, double* last_error){
+ //stand at 100
+ double error , P, I, D, PID;
- while(1){
-
- if(tsi.readPercentage()>0.00011)
- break;
- }
-
+ error=b_r_c-100;
+ P=kp*error;
+
+ I=*last_I*2.0/3.0+error;
+ *last_I=I;
+ I=ki*I;
+
+ D=error-*last_error+error;
+ *last_error=error;
+ D=kd*D;
+
+ PID=P+I+D;
-
-
- double motor;
+ if(PID<0.0){
+ PID*=-1;
+ PID=0.085-(PID*0.025);
+ }
+ else if(PID>0.0)
+ PID=0.085+(PID*0.025);
+ else
+ PID=0.085;
+
+ return PID;
+ }
+
+void run(){
+ double motor;
double b_r_c;
double PID_v;
- while(1){
-
- #ifdef task_ma_time
- task_pin=1;
- #endif
-
-
-
- cam.read();
-#ifdef Debug_cam_uart
-
- #ifdef L_eye
- for(int i=0;i<128;i++){
-
-
-
-
- if(i==64)
- pc.printf("X");
- else
- pc.printf("%c", cam.sign_line_imageL[i]);
- }
- pc.printf(" || ");
- #endif
- #ifdef R_eye
- for(int i=128;i>=0;i--){
- if(i==64)
- pc.printf("X");
-
- else if(i<10)
- pc.printf("-");
-
- else if(i>117)
-
-
- pc.printf("-");
-
- else
- pc.printf("%c", cam.sign_line_imageR[i]);
-
-
-
-
-
-
- }
- pc.printf("\r\n");
-
-
-
+ while(1){
+ #ifdef task_ma_time
+ task_pin=1;
+ #endif
+ cam.read();
+ #ifdef Debug_cam_uart
+ #ifdef L_eye
+ for(int i=0;i<128;i++){
+ if(i==64)
+ pc.printf("X");
+ else
+ pc.printf("%c", cam.sign_line_imageL[i]);
+ }
+ pc.printf(" || ");
+ #endif
+ #ifdef R_eye
+ for(int i=128;i>=0;i--){
+ if(i==64)
+ pc.printf("X");
+ else if(i<10)
+ pc.printf("-");
+ else if(i>117)
+ pc.printf("-");
+ else
+ pc.printf("%c", cam.sign_line_imageR[i]);
+ }
+ pc.printf("\r\n");
+ #endif
+ #endif
- #endif
-
-
-
-
-
-
-
-
-
-
-#endif
-
-
-
-
-
-
-
-
-
-
- #ifdef motor_on
-
-
- motor=pot1.read();
-
-
-
-
- MotorA.rotate(motor);
- MotorB.rotate(motor);
+ #ifdef motor_on
+ motor=pot1.read();
+ MotorA.rotate(motor);
+ MotorB.rotate(motor);
#endif
-
-
-
-
- b_r_c=(double)cam.black_centerR();
+ b_r_c=(double)cam.black_centerR();
- 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);
+ //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);
-
- servo.set_angle(PID_v);
+ //servo.set_angle(PID_v);
+
-
-
- #ifdef task_ma_time
- task_pin=0;
- #endif
+ //PID_v = Computes(b_r_c, &last_I, &last_error);
+ //compute
+ //stand at 100
+ double error , P, I, D;
+ double last_error=0.0, last_I=0.0;
- }
-
-
-
-
-
-
-
- return 0;
-
-
-}
+ if(error<20.0 && error>0.0)
+ PID_v=0.0;
+ else{
+ error=b_r_c-offset;
+ P=kp*error;
+
+ I=last_I*2.0/3.0+error;
+ last_I=I;
+ I=ki*I;
+
+ D=error-last_error;
+ last_error=error;
+ D=kd*D;
+
+ PID_v=P+I+D;
+ }
+
+ if(PID_v < 0.0){
+ PID_v*=-1;
+ PID_v=0.085-(PID_v/125*0.025);
+ }
+ else if(PID_v > 0.0)
+ PID_v=0.085+(PID_v/125*0.025);
+ else
+ PID_v=0.085;
+
+ servo.set_angle(PID_v);
+
+ #ifdef task_ma_time
+ task_pin=0;
+ #endif
+ }
+}
\ No newline at end of file