kao yi
/
CCCC
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 15:585df3979be8, committed 2014-06-29
- Comitter:
- backman
- Date:
- Sun Jun 29 14:02:25 2014 +0000
- Parent:
- 14:2d90b0066fc6
- Commit message:
- lin
Changed in this revision
--- a/camera_api.cpp Sat Jun 28 12:16:33 2014 +0000
+++ b/camera_api.cpp Sun Jun 29 14:02:25 2014 +0000
@@ -18,322 +18,56 @@
int l_care=10;
int r_care=118;
-
-
- // find center
- // case 1 // | //
- // case 2 / | /
-
- char find_type=0x00;
-
+ int b_start=0;
int b_end=0;
- int b_start=0;
-
+ bool l_f1=false;
+ bool l_f2=false;
+ bool find=false;
+ int b_thr_up=32;
+ int b_thr_dn=5;
int b_w=0;
- int b_center=0;
-
- int b2_end=0;
- int b2_start=0;
- int b2_center=0;
-
- int b2_w=0;
-
- int center=30;
- int j=64;
- bool f1=false;
- bool f2=false;
- bool f3=false;
- bool f4=false;
- int w_thr_up=32;
- int w_thr_dn=0;
-
- if(sign_line_imageR[64]==' ')
- find_type=0x02;
- else
- find_type=0x01;
-
-
- for(int i=64; i<r_care; i++,j--) {
-
-
- switch(find_type) {
-
-
- case 0x01:
-
- if(f1==false&&sign_line_imageR[i]==' ') {
- if(f1==false) {
- b_start=i;
- f1=true;
- }
-
- }
- if(f1== true&& f2==false&&sign_line_imageR[i]=='O') {
- if(f2==false) {
- b_end=i-1;
- f2=true;
- }
-
- }
-
- if(f3==false&&sign_line_imageR[j]==' ') {
- if(f3==false) {
- b2_end=j;
- f3=true;
- }
-
- }
- if(f3==true&&f4==false&&sign_line_imageR[j]=='O') {
- if(f4==false) {
- b2_start=j-1;
- f4=true;
- }
-
+
+ for(int i=r_care;i>l_care;i--){
+
+
+ if(l_f1==false&&sign_line_imageR[i]==' '){
+
+ b_start=i;
+ l_f1=true;
+ }
+ if(l_f1==true && sign_line_imageR[i]=='O'){
+ b_end=i-1;
+ l_f2=true;
+ }
+
+ if(l_f1==true && l_f2== true){
+ b_w=b_start-b_end;
+ if( b_thr_up>b_w&&b_w> b_thr_dn){
+
+ find=true;
+ break;
}
-
-
- break;
-
-
- case 0x02:
-
- if(sign_line_imageR[i]=='O') {
-
- if(f1==false) {
- b_end=i;
- f1=true;
- }
- }
-
- if(sign_line_imageR[j]=='O') {
-
- if(f2==false) {
- b_start=j;
- f2=true;
- }
-
- }
-
-
- break;
-
- }
-
-
+ else{
+ l_f1=false;
+ l_f2=false;
+
+ }
+
+
+ }
+
+
}
-
- b_w=b_start-b_end;
- b2_w=b2_start-b2_end;
-
- de_v=b_start;
- de_v2=b_end;
-
- switch(find_type) {
-
- case 0x01:
- b_center=(b_end+b_start)/2;
- b2_center=(b2_end+b2_start)/2;
-
-
- if(w_thr_up>b_w&&(b_center!=0)&&(b_center-64)<(64-b2_center))
- center=b_center;
- else
- center=b2_center;
-
-
-
-
-
-/* if( ( w_thr_up- (b_w))>0 &&( ( w_thr_up- (b_w)) < (w_thr_up-(b2_w)) ) ) {
- center=(b_end+b_start)/2;
-
-
-
- // } else if( ( w_thr_up- (b2_w) )>0 ) {
- center=(b2_end+b2_start)/2;
-// } else {
-
- center=65;
-
- //????????????????
-
- }
-
-*/
- break;
-
- case 0x02:
- center=(b_end+b_start)/2;
-
-
- break;
- }
-
- return center;
-
-}
-
-
+
+
+
+
-int BX_camera::black_centerL(void)
-{
-
- int l_care=10;
- int r_care=118;
-
-
- // find center
- // case 1 // | //
- // case 2 / | /
-
- char find_type=0x00;
-
- int b_end=118;
- int b_start=118;
-
- int b_w=0;
- int b_center=0;
-
- int b2_end=0;
- int b2_start=0;
- int b2_center=0;
-
- int b2_w=0;
-
- int center=30;
- int j=64;
- bool f1=false;
- bool f2=false;
- bool f3=false;
- bool f4=false;
- int w_thr_up=32;
- int w_thr_dn=0;
-
- if(sign_line_imageR[64]==' ')
- find_type=0x02;
- else
- find_type=0x01;
-
-
- for(int i=64; i<r_care; i++,j--) {
-
-
- switch(find_type) {
-
-
- case 0x01:
-
- if(f1==false&&sign_line_imageL[i]==' ') {
- if(f1==false) {
- b_start=i;
- f1=true;
- }
-
- }
- if(f1== true&& f2==false&&sign_line_imageL[i]=='O') {
- if(f2==false) {
- b_end=i-1;
- f2=true;
- }
-
- }
-
- if(f3==false&&sign_line_imageL[j]==' ') {
- if(f3==false) {
- b2_end=j;
- f3=true;
- }
-
- }
- if(f3==true&&f4==false&&sign_line_imageL[j]=='O') {
- if(f4==false) {
- b2_start=j-1;
- f4=true;
- }
-
- }
-
-
- break;
-
-
- case 0x02:
-
- if(sign_line_imageL[i]=='O') {
-
- if(f1==false) {
- b_end=i;
- f1=true;
- }
- }
-
- if(sign_line_imageL[j]=='O') {
-
- if(f2==false) {
- b_start=j;
- f2=true;
- }
-
- }
-
-
- break;
-
- }
-
-
- }
-
- b_w=b_start-b_end;
- b2_w=b2_start-b2_end;
-
- de_v=b_start;
- de_v2=b_end;
-
- switch(find_type) {
-
- case 0x01:
- b_center=(b_end+b_start)/2;
- b2_center=(b2_end+b2_start)/2;
-
-
- if(w_thr_up>b_w&&(b_center!=0)&&(b_center-64)<(64-b2_center))
- center=b_center;
- else
- center=b2_center;
-
-
-
-
-
-/* if( ( w_thr_up- (b_w))>0 &&( ( w_thr_up- (b_w)) < (w_thr_up-(b2_w)) ) ) {
- center=(b_end+b_start)/2;
-
-
-
- // } else if( ( w_thr_up- (b2_w) )>0 ) {
- center=(b2_end+b2_start)/2;
-// } else {
-
- center=65;
-
- //????????????????
-
- }
-
-*/
- break;
-
- case 0x02:
- center=(b_end+b_start)/2;
-
-
- break;
- }
-
- return (center+padding);
-
-}
+ if(find)
+ return (b_start+b_end)/2;
+ else
+ return -1;
+}
--- a/main.cpp Sat Jun 28 12:16:33 2014 +0000
+++ b/main.cpp Sun Jun 29 14:02:25 2014 +0000
@@ -8,19 +8,19 @@
#include "TSISensor.h"
#define Debug_cam_uart
-#define L_eye
+//#define L_eye
#define R_eye
#define motor_on
#define Pcontroller
#define task_ma_time
-#define car_center 15
+#define car_center 64
-#define t_cam 6
+#define t_cam 2
@@ -37,9 +37,10 @@
BX_motor MotorB('B');
BX_pot pot1('1');
+BX_pot pot2('2');
// 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);
+PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
DigitalOut task_pin(PTD1);
TSISensor tsi;
@@ -51,28 +52,31 @@
-//static int line3[3]; //special point
-static int b_r_c=0;
-static int b_l_c=118;
-
-static int l3_p=0;
+static int b_r_c=64;
+static int pre_b_r_c;
static double v_motor;
static double v_servo;
+
+
+
+
void cam_thread(void const *args){
while(true){
- cam.read();
-
+ cam.read();
- b_r_c=cam.black_centerR();
-
-
+
+ b_r_c=cam.black_centerR();
- Thread::wait(t_cam);
+ if(b_r_c==-1)
+ b_r_c=pre_b_r_c;
+
+ pre_b_r_c=b_r_c;
+ Thread::wait(t_cam);
}
@@ -114,9 +118,9 @@
else
pc.printf("%c", cam.sign_line_imageR[i]);
}
- pc.printf("\r\n");
+ pc.printf("\r\n center :%d servo: %f \r\n",cam.black_centerR(),v_servo);
#endif
- pc.printf("L: %d mid: %d R: %d center: %d \r\n", );
+
stdio_mutex.unlock();
@@ -139,13 +143,16 @@
while(1){
-
+
+
+ v_servo=cam_to_M_ctrlr.compute(b_r_c,car_center);
+
+
-
- v_servo=cam_to_M_ctrlr.compute(b_r_c,car_center);
-
- servo.set_angle(v_servo);
-
+
+ // v_servo=pot2.read();
+ servo.set_angle(v_servo);
+
Thread::wait(20);
}
@@ -153,6 +160,12 @@
}
+
+
+
+
+
+
void motor_thread(void const *args){
while(1){
@@ -169,26 +182,6 @@
}
-/*
-void ctrl_thread(void const *args){
-
-
-
- while(1){
-
-
- cam_to_M_ctrlr.compute(b_r_c,64.0);
-
- Thread::wait(1);
- }
-
-
-
-
-}
-*/
-
-// global resource
@@ -201,6 +194,7 @@
// baud rate init --- no function
+ servo.set_angle(0.055);
pc.baud(115200);
/*
while(1){
@@ -209,11 +203,15 @@
break;
}
*/
+
+
+
+
Thread th_c(cam_thread);
// Thread thread(ctrl_thread);
Thread th_s(servo_thread);
Thread th_m(motor_thread);
- // Thread th_de(de_thread);
+ Thread th_de(de_thread);
while(1){
--- a/servo_api.cpp Sat Jun 28 12:16:33 2014 +0000
+++ b/servo_api.cpp Sun Jun 29 14:02:25 2014 +0000
@@ -3,9 +3,9 @@
#include "servo_api.h"
-#define right_end 0.06 //90
+#define right_end 0.037 //90 0.037
-#define left_end 0.101 //-90
+#define left_end 0.073 //-90 0.073
//memory opt
// 5 degree seperate
@@ -31,12 +31,13 @@
float BX_servo::set_angle(float a){
-
+
+ /*
if( a>left_end )
a=left_end;
else if(a< right_end)
a=right_end;
-
+ */