Tony Lin
share
Revision 6:5a39bde2e016, committed 2014-06-11
- Comitter:
- backman
- Date:
- Wed Jun 11 14:41:52 2014 +0000
- Parent:
- 5:e32b091aa1fb
- Child:
- 7:fd976e1ced33
- Commit message:
- three;
Changed in this revision
--- a/camera_api.cpp Thu Jun 05 10:32:55 2014 +0000
+++ b/camera_api.cpp Wed Jun 11 14:41:52 2014 +0000
@@ -2,347 +2,378 @@
#include "camera_api.h"
#define clk 2 //ms
-
- BX_camera::BX_camera(void){
-
- line_CamR = new FastAnalogIn(PTD5);
- line_CamL= new FastAnalogIn(PTD6,0);
- cam_clk=new DigitalOut(PTE1);
- si=new DigitalOut(PTD7);
+
+BX_camera::BX_camera(void)
+{
+
+ line_CamR = new FastAnalogIn(PTD5);
+ line_CamL= new FastAnalogIn(PTD6,0);
+ cam_clk=new DigitalOut(PTE1);
+ si=new DigitalOut(PTD7);
+
+}
+
+int BX_camera::black_centerR(void)
+{
+
+ // find center
+ // case 1 // | //
+ // case 2 / | /
+
+ char find_type=0x00;
+
+ int b_end=0;
+ int b_start=0;
+
+ int b_w=0;
+
+
+ int b2_end=0;
+ int b2_start=0;
+
+ int b2_w=0;
+
+ int center;
+ 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<128; i++,j--) {
+
+
+ switch(find_type) {
+
+
+ case 0x01:
- }
-
-int BX_camera::black_centerR(void){
-
- // find center
- // case 1 // | //
- // case 2 / | /
-
- char find_type=0x00;
-
- int b_end=0;
- int b_start=0;
-
- int b2_end=0;
- int b2_start=0;
-
-
- int center;
- int j=64;
- bool f1=false;
- bool f2=false;
- bool f3=false;
- bool f4=false;
+ 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;
+ }
+
+ }
+
+
+ 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;
+
+ }
+
+
+ }
+
+ b_w=b_start-b_end;
+ b2_w=b2_start-b2_end;
-
- if(sign_line_imageR[64]==' ')
- find_type=0x02;
- else
- find_type=0x01;
-
-
- for(int i=64;i<128; 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;
- }
-
- }
-
-
- 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;
-
- }
-
-
- }
-
-
- switch(find_type){
-
- case 0x01:
-
- if((b_end-b_start)>(b2_end-b2_start) )
- center=(b_end+b_start)/2;
-
- else
- center=(b2_end+b2_start)/2;
- break;
-
- case 0x02:
- center=(b_end+b_start)/2;
-
- break;
- }
+ debugV=b_start;
+ debugV2=b_end;
-
- return center;
-
+
+ switch(find_type) {
+
+ case 0x01:
+
+
+
+
+
+ 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){
-
- // find center
- // case 1 // | //
- //case 2 / | /
-
-
- char find_type=0x00;
-
- int b_end=0;
- int b_start=0;
- int b2_end=0;
- int b2_start=0;
-
-
- int center;
- int j=64;
- bool f1=false;
- bool f2=false;
- bool f3=false;
- bool f4=false;
+int BX_camera::black_centerL(void)
+{
+
+ // find center
+ // case 1 // | //
+ //case 2 / | /
+
+
+ char find_type=0x00;
+
+ int b_end=0;
+ int b_start=0;
+ int b2_end=0;
+ int b2_start=0;
+
+
+ int center;
+ int j=64;
+ bool f1=false;
+ bool f2=false;
+ bool f3=false;
+ bool f4=false;
+
+ if(sign_line_imageL[64]==' ')
+ find_type=0x02;
+ else
+ find_type=0x01;
+
+
+ for(int i=64; i<128; 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(sign_line_imageL[64]==' ')
- find_type=0x02;
+ 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;
+
+ }
+
+ }
+
+
+ switch(find_type) {
+
+ case 0x01:
+
+ if((b_end-b_start)>(b2_end-b2_start) )
+ center=(b_end+b_start)/2;
+
else
- find_type=0x01;
-
-
- for(int i=64;i<128; 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;
-
- }
-
- }
-
-
- switch(find_type){
-
- case 0x01:
-
- if((b_end-b_start)>(b2_end-b2_start) )
- center=(b_end+b_start)/2;
-
- else
- center=(b2_end+b2_start)/2;
- break;
-
- case 0x02:
- center=(b_end+b_start)/2;
-
- break;
- }
-
+ center=(b2_end+b2_start)/2;
+ break;
+
+ case 0x02:
+ center=(b_end+b_start)/2;
+
+ break;
+ }
+
return center;
-
- }
-
+
+}
+
+
+
+void BX_camera::read(void)
+{
+
+ w_f_vL=0x0000;
+ b_f_vL=0xffff;
+
+ w_f_vR=0x0000;
+ b_f_vR=0xffff;
+
+
+
+
+ *si=1;
+ *cam_clk=1;
+
+ wait_us(30); // tune here
+ *si=0;
+ *cam_clk=0;
+
+
+
+ line_CamR->enable();
+ line_CamL->enable();
+
+
+ //input 128 //both
+
+ for(int i=0; i<128; i++) {
+ *cam_clk=1;
+ wait_us(5);
+
+
+ line_imageR[i]=line_CamR->read_u16();
+ line_imageL[i]=line_CamL->read_u16();
-
- void BX_camera::read(void){
-
- w_f_vL=0x0000;
- b_f_vL=0xffff;
-
- w_f_vR=0x0000;
- b_f_vR=0xffff;
-
-
-
-
- *si=1;
- *cam_clk=1;
-
- wait_us(30); // tune here
- *si=0;
- *cam_clk=0;
-
-
-
- line_CamR->enable();
- line_CamL->enable();
-
-
- //input 128 //both
-
- for(int i=0;i<128;i++){
- *cam_clk=1;
- wait_us(5);
-
-
- line_imageR[i]=line_CamR->read_u16();
- line_imageL[i]=line_CamL->read_u16();
-
- // big small
- if(line_imageR[i] > w_f_vR)
- w_f_vR=line_imageR[i];
- else if(line_imageR[i] < b_f_vR )
- b_f_vR = line_imageR[i];
-
-
- if(line_imageL[i] > w_f_vL)
- w_f_vL=line_imageL[i];
- else if(line_imageL[i] < b_f_vL )
- b_f_vL = line_imageL[i];
-
-
-
-
- *cam_clk=0;
- wait_us(5);
-
-
- }
-
-
- line_CamR->enable();
- line_CamL->enable();
-
-
- //filter L R //may change
-
- for(int i=0;i<128;i++){
-
-
- if( (line_imageR[i]-b_f_vR) < (w_f_vR - line_imageR[i] ) )
- sign_line_imageR[i]=' ';
- else
- sign_line_imageR[i]='O';
-
-
- if( (line_imageL[i]-b_f_vL) < (w_f_vL - line_imageL[i] ) )
- sign_line_imageL[i]=' ';
- else
- sign_line_imageL[i]='O';
-
-
-
-
-
- if(i==0){
- sign_line_imageR[i]='X';
- sign_line_imageL[i]='X';
- }
-
-
- }
-
-
-
- }
\ No newline at end of file
+ // big small
+ if(line_imageR[i] > w_f_vR)
+ w_f_vR=line_imageR[i];
+ else if(line_imageR[i] < b_f_vR )
+ b_f_vR = line_imageR[i];
+
+
+ if(line_imageL[i] > w_f_vL)
+ w_f_vL=line_imageL[i];
+ else if(line_imageL[i] < b_f_vL )
+ b_f_vL = line_imageL[i];
+
+
+
+
+ *cam_clk=0;
+ wait_us(5);
+
+
+ }
+
+
+ line_CamR->enable();
+ line_CamL->enable();
+
+
+ //filter L R //may change
+
+ for(int i=0; i<128; i++) {
+
+
+ if( (line_imageR[i]-b_f_vR) < (w_f_vR - line_imageR[i] ) )
+ sign_line_imageR[i]=' ';
+ else
+ sign_line_imageR[i]='O';
+
+
+ if( (line_imageL[i]-b_f_vL) < (w_f_vL - line_imageL[i] ) )
+ sign_line_imageL[i]=' ';
+ else
+ sign_line_imageL[i]='O';
+
+
+
+
+
+ if(i==0) {
+ sign_line_imageR[i]='X';
+ sign_line_imageL[i]='X';
+ }
+
+
+ }
+
+
+
+}
\ No newline at end of file
--- a/camera_api.h Thu Jun 05 10:32:55 2014 +0000
+++ b/camera_api.h Wed Jun 11 14:41:52 2014 +0000
@@ -24,6 +24,8 @@
char sign_line_imageL[128];
char sign_line_imageR[128];
+ int debugV;
+ int debugV2;
private:
--- a/main.cpp Thu Jun 05 10:32:55 2014 +0000
+++ b/main.cpp Wed Jun 11 14:41:52 2014 +0000
@@ -1,16 +1,22 @@
#include "mbed.h"
#include "servo_api.h"
#include "camera_api.h"
-
+#include "motor_api.h"
#define Debug_cam_uart
+#define R_eye
+//#define motor_on
+#define Pcontroller
+//#define servo_center
+Serial pc(USBTX, USBRX);
-Serial pc(USBTX, USBRX);
BX_servo servo;
-
- BX_camera cam;
+
+BX_camera cam;
+BX_motor MotorA('A');
+BX_motor MotorB('B');
int main() {
@@ -19,15 +25,41 @@
int black_va;
int white_va;
*/
+
+ #ifdef servo_center
+
-#ifdef Debug_cam_uart
- pc.baud(115200);
+ while(1)
+ servo.set_angle(0);
+ #endif
+
+ pc.baud(115200);
+
+ #ifdef Pcontroller
- while(1){
+ int car_center=30;
+
+ double Kp= 90.0/64.0;
- cam.read();
+ #ifdef motor_on
+ double motor=0.3;
+
+
+
+ MotorA.rotate(motor);
+ MotorB.rotate(motor);
+ #endif
+ int error=0;
+
+
+ while(1){
+ cam.read();
+
+ // #ifdef Debug_cam_uart
+
+ #ifdef L_eye
for(int i=0;i<128;i++){
if(i==64)
pc.printf("X");
@@ -35,7 +67,8 @@
pc.printf("%c", cam.sign_line_imageL[i]);
}
pc.printf(" || ");
-
+ #endif
+ // #ifdef R_eye
for(int i=0;i<128;i++){
if(i==64)
pc.printf("X");
@@ -43,34 +76,19 @@
pc.printf("%c", cam.sign_line_imageR[i]);
}
pc.printf("\r\n");
-
-
-
- pc.printf("R center : %d \r\nL center: %d\r\n",cam.black_centerR(),cam.black_centerL());
-
-
- }
-
-
-
+ // #endif
+
+
+
+
- // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
- //--------------------------------------------
-
-
- // servo.set_angle(( (64.0-center) /64.0 )*90 );
+
-
-
-
-
-
- #endif
-
+ // #endif
@@ -82,7 +100,22 @@
+
+
+ error=car_center-cam.black_centerR();
+
+
+
+
+ servo.set_angle((int)Kp*error);
+ pc.printf("b1s: %d b1e:%d b_center %d error :%d angle %d\r\n",cam.debugV,cam.debugV2,cam.black_centerR(),error,(int)Kp*error);
+
+
+ }
+ #endif
+
+