kao yi
running car
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 |
--- 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;
+
+}
-}
+
--- 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
-
}