kao yi
running car
Diff: main.cpp
- Revision:
- 11:418e39749f48
- Parent:
- 10:03d5aa2511c4
--- 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
-
}