Yi Lin Chen
shared
Diff: main.cpp
- Revision:
- 1:82bc25a7b68b
- Parent:
- 0:68c173249c01
- Child:
- 2:c51647d3c14d
--- a/main.cpp Sun May 25 12:41:59 2014 +0000
+++ b/main.cpp Tue Jun 03 15:53:55 2014 +0000
@@ -1,25 +1,311 @@
#include "mbed.h"
-
-BX_servo servo;
+#include "servo_api.h"
+#include "camera_api.h"
+//BX_servo servo;
-
-
+Serial pc(USBTX, USBRX);
+BX_servo servo;
+
+ BX_camera cam('n');
+ PwmOut cam_clk(PTC3);
int main() {
+ /*
+ int black_va;
+ int white_va;
+ */
+
+
+ pc.baud(115200);
+
+
+
+
+ char find_type=0x00;
+
+ int b_end;
+ int b_start;
+
+ int b2_end;
+ int b2_start;
+
+
+ int center;
+ int j=64;
+ bool f1;
+ bool f2;
+ bool f3;
+ bool f4;
+ while(1){
+
+
+ cam.read();
+
+ for(int i=0;i<128;i++){
+ if(i==64)
+ pc.printf("X");
+ else
+ pc.printf("%c", cam.sign_line_image[i]);
+ }
+ pc.printf("\r\n");
+
+
+
+
+ // find center
+ // case 1 // | //
+ //case 2 / | /
+
+ if(cam.sign_line_image[64]==' ')
+ find_type=0x02;
+ else
+ find_type=0x01;
+
+ b_start=0;
+ b_end=0;
+ b2_start=0;
+ b2_end=0;
+
+ j=64;
+ f1=false;
+ f2=false;
+ f3=false;
+ f4=false;
+ for(int i=64;i<128; i++,j--){
+
+
+ switch(find_type){
+
+
+ case 0x01:
+
+ if(f1==false&&cam.sign_line_image[i]==' '){
+ if(f1==false){
+ b_start=i;
+ f1=true;
+ }
+
+ }
+ if(f1== true&& f2==false&&cam.sign_line_image[i]=='O'){
+ if(f2==false){
+ b_end=i-1;
+ f2=true;
+ }
+
+ }
+
+ if(f3==false&&cam.sign_line_image[j]==' '){
+ if(f3==false){
+ b2_end=j;
+ f3=true;
+ }
+
+ }
+ if(f3==true&&f4==false&&cam.sign_line_image[j]=='O'){
+ if(f4==false){
+ b2_start=j-1;
+ f4=true;
+ }
+
+ }
+
+
+
+
+
+
+
+
+ break;
+
+
+ case 0x02:
+
+ if(cam.sign_line_image[i]=='O'){
+
+ if(f1==false){
+ b_end=i;
+ f1=true;
+ }
+ }
+
+ if(cam.sign_line_image[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;
+
+ pc.printf("%d %d | %d %d\r\n",b_start,b_end,b2_start,b2_end);
+
+
+ break;
+
+ case 0x02:
+ center=(b_end+b_start)/2;
+
+ break;
+ }
+ pc.printf("center :%d\r\n",center);
+ //--------------------------------------------
+
+
+
+
+ }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ //tmp
+ int angle;
+ float black_pR=0;
+ float black_pL=0;
+ float black_pT=0;
+ //tune
+/*
+ int min_va=90000;
+ int max_va=0;
+ int mid_va=0;
+ */
+
+
+
+ while(1){
+ black_pR=0;
+ black_pL=0;
+ black_pT=0;
+ // min_va=90000;
+ // max_va=0;
+ angle=0;
+ cam.read();
+ for(int i=0;i<64;i++){
+
+
+ // angle+=cam.line_image[i]-cam.line_image[127-i];
+ if(cam.line_image[i]==0)
+ black_pR++;
+ if(cam.line_image[127-i]==0)
+ black_pL++;
+ }
+
+
+ black_pT=black_pL+black_pR;
+
+
+ if(black_pL>black_pT/2){
+
+ angle=(black_pR/black_pT)*90;
+
+ //angle=
+ }
+ else if(black_pR>black_pT/2){
+
+ angle=-1.0*(black_pR/black_pT)*90;
+ //angle=-0.5;
+ }
+ else{
+
+ angle=0;
+
+ }
+ servo.set_angle(angle);
+ pc.printf("ang: %d",angle);
+ pc.printf("\r\n");
+
+
+
+
+
+
+
+ }
+
+
+
+
+
+
+
+ // pc.printf("mid: %d|min: %d | max: %d | diff: %d",mid_va,min_va,max_va,max_va-min_va);
+
+
+
+
+
+ //p - controller
+
+ //servo angle not yet contorl
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
+ return 0;
}