Clark Lin / Mbed 2 deprecated BX-car_s

QQQ

Dependencies:   mbed-rtos mbed

Fork of BX-car_s by Tony Lin

main.cpp@19:eb0552a0ddae, 2014-06-29 (annotated)

Committer:
TonyLin
Date:
Sun Jun 29 14:02:37 2014 +0000
Revision:
19:eb0552a0ddae
Parent:
18:88b083db7491
Child:
20:4ed21397e775
new;

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
TonyLin 14:303b22b76d7a 11 #define motor_on
backman 6:5a39bde2e016 12 #define Pcontroller
backman 8:8e49e21d80a2 13 #define task_ma_time
TonyLin 13:a33a7705fe2b 14 #define offset 65
TonyLin 13:a33a7705fe2b 15 #include "TSISensor.h"
backman 12:418e39749f48 16
TonyLin 13:a33a7705fe2b 17 Serial pc(USBTX, USBRX);
TonyLin 14:303b22b76d7a 18 BX_servo servo;
backman 6:5a39bde2e016 19 BX_camera cam;
backman 6:5a39bde2e016 20 BX_motor MotorA('A');
backman 6:5a39bde2e016 21 BX_motor MotorB('B');
TonyLin 13:a33a7705fe2b 22 BX_pot pot1('1'); // 90/30=3
TonyLin 13:a33a7705fe2b 23 BX_pot pot2('2');
TonyLin 13:a33a7705fe2b 24 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
backman 8:8e49e21d80a2 25 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 26 TSISensor tsi;
backman 12:418e39749f48 27
TonyLin 13:a33a7705fe2b 28 void run();
backman 12:418e39749f48 29
TonyLin 14:303b22b76d7a 30 int main()
TonyLin 14:303b22b76d7a 31 {
TonyLin 13:a33a7705fe2b 32 pc.baud(115200);
TonyLin 14:303b22b76d7a 33
TonyLin 15:57567d3ee27e 34 /*while(1) {
TonyLin 13:a33a7705fe2b 35 if(tsi.readPercentage()>0.00011)
TonyLin 14:303b22b76d7a 36 break;
TonyLin 15:57567d3ee27e 37 }*/
TonyLin 13:a33a7705fe2b 38 run();
TonyLin 14:303b22b76d7a 39
TonyLin 13:a33a7705fe2b 40 return 0;
TonyLin 14:303b22b76d7a 41 }
backman 12:418e39749f48 42
backman 12:418e39749f48 43
backman 7:fd976e1ced33 44
TonyLin 14:303b22b76d7a 45 void run()
TonyLin 14:303b22b76d7a 46 {
TonyLin 15:57567d3ee27e 47 double motor, angle=0.0;
TonyLin 14:303b22b76d7a 48 double b_r_c;
TonyLin 14:303b22b76d7a 49 double PID_v;
TonyLin 14:303b22b76d7a 50 double error , P, I, D, kp, r_kp;
TonyLin 14:303b22b76d7a 51 double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
TonyLin 14:303b22b76d7a 52 bool first_time=true, r_kp_neg=false;
TonyLin 14:303b22b76d7a 53
TonyLin 14:303b22b76d7a 54 while(1) {
TonyLin 14:303b22b76d7a 55 #ifdef task_ma_time
TonyLin 14:303b22b76d7a 56 task_pin=1;
TonyLin 14:303b22b76d7a 57 #endif
TonyLin 14:303b22b76d7a 58
TonyLin 14:303b22b76d7a 59 cam.read();
TonyLin 14:303b22b76d7a 60 #ifdef Debug_cam_uart
TonyLin 14:303b22b76d7a 61 #ifdef L_eye
TonyLin 14:303b22b76d7a 62 for(int i=0; i<128; i++) {
TonyLin 14:303b22b76d7a 63 if(i==64)
TonyLin 14:303b22b76d7a 64 pc.printf("X");
TonyLin 14:303b22b76d7a 65 else
TonyLin 14:303b22b76d7a 66 pc.printf("%c", cam.sign_line_imageL[i]);
TonyLin 14:303b22b76d7a 67 }
TonyLin 14:303b22b76d7a 68 pc.printf(" || ");
TonyLin 14:303b22b76d7a 69 #endif
TonyLin 14:303b22b76d7a 70 #ifdef R_eye
TonyLin 14:303b22b76d7a 71 for(int i=128; i>=0; i--) {
TonyLin 14:303b22b76d7a 72 if(i==64)
TonyLin 14:303b22b76d7a 73 pc.printf("X");
TonyLin 14:303b22b76d7a 74 else if(i<10)
TonyLin 14:303b22b76d7a 75 pc.printf("-");
TonyLin 14:303b22b76d7a 76 else if(i>117)
TonyLin 14:303b22b76d7a 77 pc.printf("-");
TonyLin 14:303b22b76d7a 78 else
TonyLin 14:303b22b76d7a 79 pc.printf("%c", cam.sign_line_imageR[i]);
TonyLin 14:303b22b76d7a 80 }
TonyLin 19:eb0552a0ddae 81 pc.printf(" ");
TonyLin 19:eb0552a0ddae 82 //pc.printf("\r\n");
TonyLin 14:303b22b76d7a 83 #endif
TonyLin 14:303b22b76d7a 84 #endif
TonyLin 14:303b22b76d7a 85
TonyLin 14:303b22b76d7a 86 #ifdef motor_on
TonyLin 14:303b22b76d7a 87 motor=pot1.read();
TonyLin 15:57567d3ee27e 88 MotorA.rotate(0.0);
TonyLin 15:57567d3ee27e 89 MotorB.rotate(0.0);
TonyLin 14:303b22b76d7a 90 #endif
TonyLin 14:303b22b76d7a 91
TonyLin 14:303b22b76d7a 92 b_r_c=(double)cam.black_centerR();
TonyLin 15:57567d3ee27e 93 //pc.printf("%d %d speed :%f bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
TonyLin 14:303b22b76d7a 94
TonyLin 14:303b22b76d7a 95 //compute
TonyLin 14:303b22b76d7a 96 //stand at 65
TonyLin 14:303b22b76d7a 97 error=b_r_c-offset;
TonyLin 14:303b22b76d7a 98 brc_df=b_r_c-last_brc;
TonyLin 19:eb0552a0ddae 99
backman 12:418e39749f48 100
TonyLin 14:303b22b76d7a 101 if(first_time==true){
TonyLin 17:af867c7512bb 102 r_kp=0;
TonyLin 14:303b22b76d7a 103 first_time=false;
TonyLin 13:a33a7705fe2b 104 }
TonyLin 13:a33a7705fe2b 105 else{
TonyLin 15:57567d3ee27e 106 r_kp+=brc_df;
TonyLin 19:eb0552a0ddae 107 if(b_r_c==last_brc)
TonyLin 19:eb0552a0ddae 108 r_kp=0;
TonyLin 14:303b22b76d7a 109 }
TonyLin 19:eb0552a0ddae 110 last_brc=b_r_c;
backman 12:418e39749f48 111
TonyLin 14:303b22b76d7a 112 if(r_kp<0.0){
TonyLin 14:303b22b76d7a 113 r_kp*=-1.0;
TonyLin 14:303b22b76d7a 114 r_kp_neg=true;
TonyLin 14:303b22b76d7a 115 }
TonyLin 19:eb0552a0ddae 116 kp=0.016/(30-(r_kp/10.0));
TonyLin 14:303b22b76d7a 117 if(r_kp_neg==true){
TonyLin 14:303b22b76d7a 118 r_kp_neg=false;
TonyLin 14:303b22b76d7a 119 r_kp*=-1.0;
TonyLin 14:303b22b76d7a 120 }
TonyLin 14:303b22b76d7a 121
TonyLin 14:303b22b76d7a 122 P=kp*error;
TonyLin 14:303b22b76d7a 123
TonyLin 14:303b22b76d7a 124 I=last_I*2.0/3.0+error;
TonyLin 14:303b22b76d7a 125 last_I=I;
TonyLin 14:303b22b76d7a 126 I=(kp*0.02/0.04)*I;
TonyLin 14:303b22b76d7a 127
TonyLin 14:303b22b76d7a 128 D=error-last_error;
TonyLin 14:303b22b76d7a 129 last_error=error;
TonyLin 14:303b22b76d7a 130 D=(kp*0.04/0.02)*D;
TonyLin 14:303b22b76d7a 131
TonyLin 15:57567d3ee27e 132 PID_v=P;
TonyLin 14:303b22b76d7a 133
TonyLin 14:303b22b76d7a 134 if(PID_v < 0.0) {
TonyLin 15:57567d3ee27e 135 PID_v*=-1.0;
TonyLin 19:eb0552a0ddae 136 PID_v=0.077+PID_v;
TonyLin 14:303b22b76d7a 137 } else if(PID_v > 0.0)
TonyLin 19:eb0552a0ddae 138 PID_v=0.077-PID_v;
TonyLin 13:a33a7705fe2b 139 else
TonyLin 15:57567d3ee27e 140 PID_v=0.077;
TonyLin 16:b1e11b865d05 141 pc.printf("%lf %lf %lf\r\n",PID_v, r_kp, b_r_c);
TonyLin 15:57567d3ee27e 142 //pc.printf("%lf\r\n",angle);
TonyLin 13:a33a7705fe2b 143 servo.set_angle(PID_v);
TonyLin 14:303b22b76d7a 144
TonyLin 14:303b22b76d7a 145 #ifdef task_ma_time
TonyLin 14:303b22b76d7a 146 task_pin=0;
TonyLin 14:303b22b76d7a 147 #endif
TonyLin 14:303b22b76d7a 148 }
TonyLin 13:a33a7705fe2b 149 }