chia-hsun wu
/
Boboobooov10
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.
Fork of
Boboobooov4
by 
kao yi
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;
}