kao yi
/
BX-car
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 11:418e39749f48, committed 2014-06-28
- Comitter:
- backman
- Date:
- Sat Jun 28 05:43:23 2014 +0000
- Parent:
- 10:03d5aa2511c4
- Commit message:
- wang
Changed in this revision
| camera_api.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 03d5aa2511c4 -r 418e39749f48 camera_api.cpp
--- a/camera_api.cpp Thu Jun 26 09:15:35 2014 +0000
+++ b/camera_api.cpp Sat Jun 28 05:43:23 2014 +0000
@@ -175,37 +175,47 @@
-
int BX_camera::black_centerL(void)
{
+ int l_care=10;
+ int r_care=118;
+
+
// find center
// case 1 // | //
- //case 2 / | /
-
+ // case 2 / | /
char find_type=0x00;
- int b_end=0;
- int b_start=0;
+ 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 center;
+ 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_imageL[64]==' ')
+ if(sign_line_imageR[64]==' ')
find_type=0x02;
else
find_type=0x01;
- for(int i=64; i<128; i++,j--) {
+ for(int i=64; i<r_care; i++,j--) {
switch(find_type) {
@@ -213,14 +223,14 @@
case 0x01:
- if(f1==false&&sign_line_imageL[i]==' ') {
+ if(f1==false&&sign_line_imageR[i]==' ') {
if(f1==false) {
b_start=i;
f1=true;
}
}
- if(f1== true&& f2==false&&sign_line_imageL[i]=='O') {
+ if(f1== true&& f2==false&&sign_line_imageR[i]=='O') {
if(f2==false) {
b_end=i-1;
f2=true;
@@ -228,14 +238,14 @@
}
- if(f3==false&&sign_line_imageL[j]==' ') {
+ if(f3==false&&sign_line_imageR[j]==' ') {
if(f3==false) {
b2_end=j;
f3=true;
}
}
- if(f3==true&&f4==false&&sign_line_imageL[j]=='O') {
+ if(f3==true&&f4==false&&sign_line_imageR[j]=='O') {
if(f4==false) {
b2_start=j-1;
f4=true;
@@ -243,12 +253,13 @@
}
+
break;
case 0x02:
- if(sign_line_imageL[i]=='O') {
+ if(sign_line_imageR[i]=='O') {
if(f1==false) {
b_end=i;
@@ -256,7 +267,7 @@
}
}
- if(sign_line_imageL[j]=='O') {
+ if(sign_line_imageR[j]=='O') {
if(f2==false) {
b_start=j;
@@ -270,29 +281,61 @@
}
+
}
+ 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((b_end-b_start)>(b2_end-b2_start) )
+
+
+
+
+/* if( ( w_thr_up- (b_w))>0 &&( ( w_thr_up- (b_w)) < (w_thr_up-(b2_w)) ) ) {
center=(b_end+b_start)/2;
- else
+
+
+ // } 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;
+
+}
-}
+
diff -r 03d5aa2511c4 -r 418e39749f48 main.cpp
--- a/main.cpp Thu Jun 26 09:15:35 2014 +0000
+++ b/main.cpp Sat Jun 28 05:43:23 2014 +0000
@@ -1,19 +1,27 @@
#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
-#include "TSISensor.h"
+
+#define t_cam 6
+
+
-Serial pc(USBTX, USBRX);
+
+
+
+
BX_servo servo;
@@ -30,133 +38,220 @@
DigitalOut task_pin(PTD1);
TSISensor tsi;
-int main() {
-
-
- pc.baud(115200);
+
+//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 cam_thread(void const *args){
- while(1){
-
- if(tsi.readPercentage()>0.00011)
- break;
- }
-
+ 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){
-
+ 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();
- double motor;
- double b_r_c;
- double PID_v;
- while(1){
- #ifdef task_ma_time
- task_pin=1;
- #endif
-
+
+
- cam.read();
-
-#ifdef Debug_cam_uart
-
- #ifdef L_eye
- for(int i=0;i<128;i++){
+#endif
+ Thread::wait(1);
-
-
-
- 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]);
-
-
+void servo_thread(void const *args){
+
+
+ while(1){
+
+ int ctrl=(line3[0]+line3[2])/2;
+
+
+ v_servo=cam_to_M_ctrlr.compute(ctrl,118.0);
+ servo.set_angle(v_servo);
+
+ Thread::wait(20);
+ }
+
+
+
+}
+
+void motor_thread(void const *args){
+
+ while(1){
-
-
- }
- pc.printf("\r\n");
-
-
-
-
- #endif
-
-
-
+ MotorA.rotate(v_motor);
+ MotorB.rotate(v_motor);
-
-
-
-
-
-
-#endif
-
+ Thread::wait(1);
+ }
+
+
-
-
-
-
-
-
-
-
- #ifdef motor_on
-
-
- motor=pot1.read();
+
+ }
+
+
+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
-
-
- MotorA.rotate(motor);
- MotorB.rotate(motor);
- #endif
-
-
-
-
-
- b_r_c=(double)cam.black_centerR();
-
- PID_v=cam_to_M_ctrlr.compute(b_r_c,64.0);
-
+ /*
+ while(1){
-
- #ifdef Debug_cam_uart
-
- pc.printf("cam %d %d k:%f i:%f d:%f speed :%f bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,cam_to_M_ctrlr.de_kp,cam_to_M_ctrlr.de_ip,cam_to_M_ctrlr.de_dp,motor,b_r_c,PID_v);
+ 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){
- #endif
- servo.set_angle(PID_v);
-
-
-
+
+ //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);
- #ifdef task_ma_time
- task_pin=0;
- #endif
-
}