Clark Lin
QQQ
Diff: main.cpp
- Revision:
- 13:a33a7705fe2b
- Parent:
- 12:418e39749f48
- Child:
- 14:303b22b76d7a
--- a/main.cpp Sat Jun 28 05:43:23 2014 +0000
+++ b/main.cpp Sat Jun 28 07:06:54 2014 +0000
@@ -1,266 +1,130 @@
#include "mbed.h"
-#include "rtos.h"
#include "controller.h"
#include "servo_api.h"
#include "camera_api.h"
#include "motor_api.h"
#include "pot.h"
-#include "TSISensor.h"
+
#define Debug_cam_uart
-#define L_eye
#define R_eye
#define motor_on
#define Pcontroller
#define task_ma_time
-
-#define t_cam 6
-
-
-
-
-
+#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
-PID cam_to_M_ctrlr(10.0,118.0,0.059,0.105,0.105-0.059,0.00,0.02,10);
-
+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;
-//os
-Mutex stdio_mutex;
-
-
-/*
-void led2_thread(void const *args) {
- while (true) {
- led2 = !led2;
- Thread::wait(1000);
- }
-}
-*/
-
-static int b_r_c=0;
-static int b_l_c=0;
-static int line3[3]; //special point
-static int l3_p=0;
-
-static double v_motor;
-static double v_servo;
+void run();
-void cam_thread(void const *args){
-
- while(true){
- cam.read();
-
- b_l_c=(double)cam.black_centerL();
-
- if(l3_p==0){
-
- line3[l3_p]=(double)cam.black_centerR();
- }else if(l3_p==1){
- line3[l3_p]=0;
-
- }else {
- line3[l3_p]=(double)cam.black_centerL();
-
- }
-
- l3_p++;
- if(l3_p==3)
- l3_p=0;
-
- Thread::wait(t_cam);
-
- }
-
-}
-// function
-void de_thread(void const *args){
+int main(){
+ pc.baud(115200);
- while(1){
-
-
- cam.read();
-
- b_l_c=(double)cam.black_centerL();
-
- if(l3_p==0){
-
- line3[l3_p]=(double)cam.black_centerR();
- }else if(l3_p==1){
- line3[l3_p]=0;
-
- }else {
- line3[l3_p]=(double)cam.black_centerL();
-
- }
-
- l3_p++;
- if(l3_p==3)
- l3_p=0;
-
-
- stdio_mutex.lock();
- #ifdef Debug_cam_uart
- #ifdef L_eye
- printf("L: ");
- for(int i=0;i<128;i++){
- if(i==64)
- printf("X");
- else if(i<10)
- printf("-");
- else if(i>117)
- printf("-");
-
- else
- printf("%c", cam.sign_line_imageL[i]);
- }
- printf(" || ");
- #endif
- #ifdef R_eye
- printf("R: ");
- for(int i=128;i>=0;i--){
- if(i==64)
- printf("X");
- else if(i<10)
- printf("-");
- else if(i>117)
- printf("-");
- else
- printf("%c", cam.sign_line_imageR[i]);
- }
- printf("\r\n");
- #endif
- printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
- stdio_mutex.unlock();
-
-
-
-
-
-
-#endif
- Thread::wait(1);
-
-
+ while(1){
+ if(tsi.readPercentage()>0.00011)
+ break;
}
-
-}
+ run();
+
+ pc.baud(115200);
+ return 0;
+ }
-
-void servo_thread(void const *args){
-
-
- while(1){
-
- int ctrl=(line3[0]+line3[2])/2;
-
+void run(){
+ double motor;
+ double b_r_c;
+ double PID_v;
+
+ while(1){
+ #ifdef task_ma_time
+ task_pin=1;
+ #endif
- v_servo=cam_to_M_ctrlr.compute(ctrl,118.0);
- servo.set_angle(v_servo);
+ 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
+
+ #ifdef motor_on
+ motor=pot1.read();
+ MotorA.rotate(motor);
+ MotorB.rotate(motor);
+ #endif
+
+ b_r_c=(double)cam.black_centerR();
+ pc.printf("%d %d speed :%f bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
+
+ //compute
+ //stand at 100
+ double error , P, I, D;
+ double last_error=0.0, last_I=0.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;
- Thread::wait(20);
- }
-
-
-
-}
-
-void motor_thread(void const *args){
-
- while(1){
+ D=error-last_error;
+ last_error=error;
+ D=kd*D;
-
- MotorA.rotate(v_motor);
- MotorB.rotate(v_motor);
-
- Thread::wait(1);
+ PID_v=P+I+D;
}
-
-
-
-
- }
-
-
-void ctrl_thread(void const *args){
-
-
-
- while(1){
-
- b_r_c=(double)cam.black_centerR();
-
- cam_to_M_ctrlr.compute(b_r_c,64.0);
-
- Thread::wait(1);
- }
-
-
-
-
-}
-
-
-// global resource
-
-
-
-
-
-
-
-
-int main() {
-
-// baud rate init --- no function
-
-
- /*
- while(1){
-
- if(tsi.readPercentage()>0.00011)
- break;
- }
- */
- // Thread th_c(cam_thread);
- // Thread thread(ctrl_thread);
- // Thread thread(servo_thread);
- // Thread thread(motor_thread);
- Thread th_de(de_thread);
- while(1){
-
-
- //idle
- //stdio_mutex.lock();
- // printf("L: %d mid: %d R: %d\r\n",line3[0],line3[1],line3[2]);
- // stdio_mutex.unlock();
- Thread::wait(2000);
-
+
+ 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
}
-
-
-
-
-
-
-
- return 0;
-
-
-}
+}
\ No newline at end of file