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:
- 11:418e39749f48
- Parent:
- 10:03d5aa2511c4
- Child:
- 12:fdada4af384a
--- 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
-
}