Clark Lin / Mbed 2 deprecated BX-car_s

QQQ

Dependencies:   mbed-rtos mbed

Fork of BX-car_s by Tony Lin

main.cpp@22:1464a3f0a290, 2014-07-01 (annotated)

Committer:
physicsgood
Date:
Tue Jul 01 13:09:06 2014 +0000
Revision:
22:1464a3f0a290
Parent:
21:5f7efc1ca8ad
Child:
23:d6d4e8adf7fe
QQQQ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
backman 7:fd976e1ced33 2 #include "controller.h"
backman 1:82bc25a7b68b 3 #include "servo_api.h"
backman 1:82bc25a7b68b 4 #include "camera_api.h"
backman 6:5a39bde2e016 5 #include "motor_api.h"
backman 7:fd976e1ced33 6 #include "pot.h"
TonyLin 13:a33a7705fe2b 7
backman 3:c5f2281b3ed2 8
TonyLin 16:b1e11b865d05 9 #define Debug_cam_uart
backman 6:5a39bde2e016 10 #define R_eye
physicsgood 21:5f7efc1ca8ad 11 #define L_eye
TonyLin 14:303b22b76d7a 12 #define motor_on
backman 6:5a39bde2e016 13 #define Pcontroller
backman 8:8e49e21d80a2 14 #define task_ma_time
TonyLin 13:a33a7705fe2b 15 #define offset 65
TonyLin 13:a33a7705fe2b 16 #include "TSISensor.h"
backman 12:418e39749f48 17
TonyLin 13:a33a7705fe2b 18 Serial pc(USBTX, USBRX);
TonyLin 14:303b22b76d7a 19 BX_servo servo;
TonyLin 20:4ed21397e775 20 BX_camera cam(0);
backman 6:5a39bde2e016 21 BX_motor MotorA('A');
backman 6:5a39bde2e016 22 BX_motor MotorB('B');
TonyLin 13:a33a7705fe2b 23 BX_pot pot1('1'); // 90/30=3
TonyLin 13:a33a7705fe2b 24 BX_pot pot2('2');
TonyLin 13:a33a7705fe2b 25 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
backman 8:8e49e21d80a2 26 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 27 TSISensor tsi;
backman 12:418e39749f48 28
TonyLin 13:a33a7705fe2b 29 void run();
backman 12:418e39749f48 30
TonyLin 14:303b22b76d7a 31 int main()
TonyLin 14:303b22b76d7a 32 {
TonyLin 13:a33a7705fe2b 33 pc.baud(115200);
TonyLin 14:303b22b76d7a 34
TonyLin 15:57567d3ee27e 35 /*while(1) {
TonyLin 13:a33a7705fe2b 36 if(tsi.readPercentage()>0.00011)
TonyLin 14:303b22b76d7a 37 break;
TonyLin 15:57567d3ee27e 38 }*/
TonyLin 13:a33a7705fe2b 39 run();
TonyLin 14:303b22b76d7a 40
TonyLin 13:a33a7705fe2b 41 return 0;
TonyLin 14:303b22b76d7a 42 }
backman 12:418e39749f48 43
backman 12:418e39749f48 44
backman 7:fd976e1ced33 45
TonyLin 14:303b22b76d7a 46 void run()
TonyLin 14:303b22b76d7a 47 {
TonyLin 15:57567d3ee27e 48 double motor, angle=0.0;
physicsgood 22:1464a3f0a290 49 int centerR,centerL;
TonyLin 14:303b22b76d7a 50 double PID_v;
TonyLin 14:303b22b76d7a 51 double error , P, I, D, kp, r_kp;
TonyLin 14:303b22b76d7a 52 double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
TonyLin 14:303b22b76d7a 53 bool first_time=true, r_kp_neg=false;
TonyLin 14:303b22b76d7a 54
TonyLin 14:303b22b76d7a 55 while(1) {
TonyLin 14:303b22b76d7a 56 #ifdef task_ma_time
TonyLin 14:303b22b76d7a 57 task_pin=1;
TonyLin 14:303b22b76d7a 58 #endif
TonyLin 14:303b22b76d7a 59
TonyLin 14:303b22b76d7a 60 cam.read();
TonyLin 14:303b22b76d7a 61 #ifdef Debug_cam_uart
TonyLin 14:303b22b76d7a 62 #ifdef L_eye
physicsgood 22:1464a3f0a290 63 pc.printf("X");
physicsgood 22:1464a3f0a290 64 for(int i=122; i>=64; i--) {
physicsgood 22:1464a3f0a290 65 if(i==64)
TonyLin 14:303b22b76d7a 66 pc.printf("X");
TonyLin 14:303b22b76d7a 67 else
TonyLin 14:303b22b76d7a 68 pc.printf("%c", cam.sign_line_imageL[i]);
TonyLin 14:303b22b76d7a 69 }
physicsgood 21:5f7efc1ca8ad 70 pc.printf(" || ");
TonyLin 14:303b22b76d7a 71 #endif
TonyLin 14:303b22b76d7a 72 #ifdef R_eye
physicsgood 22:1464a3f0a290 73 for(int i=64; i>=6; i--) {
TonyLin 14:303b22b76d7a 74 if(i==64)
TonyLin 14:303b22b76d7a 75 pc.printf("X");
TonyLin 14:303b22b76d7a 76 else
TonyLin 14:303b22b76d7a 77 pc.printf("%c", cam.sign_line_imageR[i]);
TonyLin 14:303b22b76d7a 78 }
physicsgood 22:1464a3f0a290 79 pc.printf("X");
physicsgood 21:5f7efc1ca8ad 80 pc.printf("\r\n");
TonyLin 14:303b22b76d7a 81 #endif
TonyLin 14:303b22b76d7a 82 #endif
TonyLin 14:303b22b76d7a 83
TonyLin 14:303b22b76d7a 84 #ifdef motor_on
TonyLin 14:303b22b76d7a 85 motor=pot1.read();
TonyLin 20:4ed21397e775 86 MotorA.rotate(motor);
TonyLin 20:4ed21397e775 87 MotorB.rotate(motor);
TonyLin 20:4ed21397e775 88 //pc.printf("%f\r\n",motor);
TonyLin 14:303b22b76d7a 89 #endif
TonyLin 14:303b22b76d7a 90
physicsgood 22:1464a3f0a290 91 centerR=cam.black_centerR();
physicsgood 22:1464a3f0a290 92 centerL=cam.black_centerL();
physicsgood 22:1464a3f0a290 93 if(centerR == 0 && centerL ==128){
physicsgood 22:1464a3f0a290 94 servo.set_angle(0.073);
physicsgood 22:1464a3f0a290 95 }
physicsgood 22:1464a3f0a290 96 else if(centerR == 0 && centerL !=128){//turn right
physicsgood 22:1464a3f0a290 97 PID_v=cam_to_M_ctrlr.compute(centerL,120);//set_angle()
physicsgood 22:1464a3f0a290 98 servo.set_angle(PID_v);//122~64
physicsgood 22:1464a3f0a290 99 }
physicsgood 22:1464a3f0a290 100 else if(centerR != 0 && centerL ==128){//turn left
physicsgood 22:1464a3f0a290 101 PID_v=cam_to_M_ctrlr.compute(centerR,8);//set_angle()
physicsgood 22:1464a3f0a290 102 servo.set_angle(PID_v);//64~6 64
physicsgood 22:1464a3f0a290 103 }
physicsgood 22:1464a3f0a290 104 else{
physicsgood 22:1464a3f0a290 105 PID_v=cam_to_M_ctrlr.compute((centerR+centerL)/2,64);//set_angle()
physicsgood 22:1464a3f0a290 106 servo.set_angle(PID_v);//64~6 64
physicsgood 22:1464a3f0a290 107 }
physicsgood 22:1464a3f0a290 108 pc.printf("centerL: %d centerR: %d",centerL,centerR);
physicsgood 21:5f7efc1ca8ad 109 pc.printf("\r\n");
physicsgood 21:5f7efc1ca8ad 110 //pc.printf("angle: %f\r\n",PID_v);
physicsgood 21:5f7efc1ca8ad 111 //pc.printf("%f %d %d speed :%f bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,center,PID_v);
physicsgood 21:5f7efc1ca8ad 112 //center range 0~128
physicsgood 21:5f7efc1ca8ad 113 last_error = error;
TonyLin 14:303b22b76d7a 114 #ifdef task_ma_time
TonyLin 14:303b22b76d7a 115 task_pin=0;
TonyLin 14:303b22b76d7a 116 #endif
TonyLin 14:303b22b76d7a 117 }
TonyLin 13:a33a7705fe2b 118 }