譯文 張 / Mbed 2 deprecated BX-car

good

Dependencies:   mbed

Fork of BX-car by Clark Lin

main.cpp@11:ffd762ae141b, 2014-06-25 (annotated)

Committer:
even
Date:
Wed Jun 25 05:59:29 2014 +0000
Revision:
11:ffd762ae141b
Parent:
10:d2401a243e8d 
1st PID complete

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"
backman 7:fd976e1ced33 7
backman 3:c5f2281b3ed2 8
backman 3:c5f2281b3ed2 9 #define Debug_cam_uart
backman 6:5a39bde2e016 10 #define R_eye
backman 7:fd976e1ced33 11 #define motor_on
backman 6:5a39bde2e016 12 #define Pcontroller
backman 8:8e49e21d80a2 13 #define task_ma_time
backman 9:33b99cb45e99 14 #include "TSISensor.h"
backman 7:fd976e1ced33 15
backman 6:5a39bde2e016 16 Serial pc(USBTX, USBRX);
backman 0:68c173249c01 17
backman 0:68c173249c01 18
backman 1:82bc25a7b68b 19 BX_servo servo;
backman 6:5a39bde2e016 20
backman 6:5a39bde2e016 21 BX_camera cam;
backman 0:68c173249c01 22
backman 6:5a39bde2e016 23 BX_motor MotorA('A');
backman 6:5a39bde2e016 24 BX_motor MotorB('B');
backman 3:c5f2281b3ed2 25
backman 7:fd976e1ced33 26 BX_pot pot1('1');
backman 7:fd976e1ced33 27
backman 9:33b99cb45e99 28 // 90/30=3
backman 9:33b99cb45e99 29 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
physicsgood 10:d2401a243e8d 30 //PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval)
physicsgood 10:d2401a243e8d 31 //in_min in_out camera array
physicsgood 10:d2401a243e8d 32 //out_min out_max servo range
physicsgood 10:d2401a243e8d 33 //
backman 7:fd976e1ced33 34
backman 8:8e49e21d80a2 35 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 36 TSISensor tsi;
backman 0:68c173249c01 37 int main() {
backman 0:68c173249c01 38
backman 9:33b99cb45e99 39
backman 9:33b99cb45e99 40 pc.baud(115200);
backman 9:33b99cb45e99 41 while(1){
backman 9:33b99cb45e99 42
backman 9:33b99cb45e99 43 if(tsi.readPercentage()>0.00011)
backman 9:33b99cb45e99 44 break;
backman 9:33b99cb45e99 45 }
backman 1:82bc25a7b68b 46
backman 9:33b99cb45e99 47
backman 6:5a39bde2e016 48
backman 1:82bc25a7b68b 49
backman 7:fd976e1ced33 50 double motor;
backman 8:8e49e21d80a2 51 double b_r_c;
backman 8:8e49e21d80a2 52 double PID_v;
even 11:ffd762ae141b 53
even 11:ffd762ae141b 54 double Kp = 0.0005, Ki = 0.0001, Kd = 0.0001;
even 11:ffd762ae141b 55 double integral = 0, derivative = 0, error, last_error = 0, turn;
even 11:ffd762ae141b 56
backman 6:5a39bde2e016 57 while(1){
backman 1:82bc25a7b68b 58
backman 7:fd976e1ced33 59 #ifdef task_ma_time
backman 7:fd976e1ced33 60 task_pin=1;
backman 7:fd976e1ced33 61 #endif
backman 7:fd976e1ced33 62
backman 7:fd976e1ced33 63
backman 7:fd976e1ced33 64
backman 6:5a39bde2e016 65 cam.read();
backman 6:5a39bde2e016 66
backman 7:fd976e1ced33 67 #ifdef Debug_cam_uart
backman 7:fd976e1ced33 68
backman 6:5a39bde2e016 69 #ifdef L_eye
backman 1:82bc25a7b68b 70 for(int i=0;i<128;i++){
backman 7:fd976e1ced33 71
backman 7:fd976e1ced33 72
backman 7:fd976e1ced33 73
backman 7:fd976e1ced33 74
backman 1:82bc25a7b68b 75 if(i==64)
backman 1:82bc25a7b68b 76 pc.printf("X");
backman 1:82bc25a7b68b 77 else
backman 2:c51647d3c14d 78 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 79 }
backman 2:c51647d3c14d 80 pc.printf(" || ");
backman 7:fd976e1ced33 81 #endif
backman 7:fd976e1ced33 82 #ifdef R_eye
backman 7:fd976e1ced33 83 for(int i=128;i>=0;i--){
backman 2:c51647d3c14d 84 if(i==64)
backman 2:c51647d3c14d 85 pc.printf("X");
backman 7:fd976e1ced33 86
backman 7:fd976e1ced33 87 else if(i<10)
backman 7:fd976e1ced33 88 pc.printf("-");
backman 7:fd976e1ced33 89
backman 7:fd976e1ced33 90 else if(i>117)
backman 7:fd976e1ced33 91
backman 7:fd976e1ced33 92
backman 7:fd976e1ced33 93 pc.printf("-");
backman 7:fd976e1ced33 94
backman 2:c51647d3c14d 95 else
backman 2:c51647d3c14d 96 pc.printf("%c", cam.sign_line_imageR[i]);
backman 7:fd976e1ced33 97
backman 7:fd976e1ced33 98
backman 7:fd976e1ced33 99
backman 7:fd976e1ced33 100
backman 7:fd976e1ced33 101
backman 7:fd976e1ced33 102
backman 1:82bc25a7b68b 103 }
backman 1:82bc25a7b68b 104 pc.printf("\r\n");
backman 6:5a39bde2e016 105
backman 6:5a39bde2e016 106
backman 6:5a39bde2e016 107
backman 3:c5f2281b3ed2 108
backman 7:fd976e1ced33 109 #endif
backman 7:fd976e1ced33 110
backman 7:fd976e1ced33 111
backman 3:c5f2281b3ed2 112
backman 7:fd976e1ced33 113
backman 3:c5f2281b3ed2 114
backman 3:c5f2281b3ed2 115
backman 6:5a39bde2e016 116
backman 3:c5f2281b3ed2 117
backman 6:5a39bde2e016 118
backman 2:c51647d3c14d 119
backman 7:fd976e1ced33 120 #endif
backman 1:82bc25a7b68b 121
backman 7:fd976e1ced33 122
backman 1:82bc25a7b68b 123
backman 1:82bc25a7b68b 124
backman 1:82bc25a7b68b 125
backman 1:82bc25a7b68b 126
backman 1:82bc25a7b68b 127
backman 6:5a39bde2e016 128
backman 6:5a39bde2e016 129
backman 7:fd976e1ced33 130
backman 7:fd976e1ced33 131 #ifdef motor_on
backman 7:fd976e1ced33 132
backman 7:fd976e1ced33 133
backman 7:fd976e1ced33 134 motor=pot1.read();
backman 6:5a39bde2e016 135
backman 7:fd976e1ced33 136
backman 1:82bc25a7b68b 137
backman 7:fd976e1ced33 138
backman 7:fd976e1ced33 139 MotorA.rotate(motor);
backman 7:fd976e1ced33 140 MotorB.rotate(motor);
backman 7:fd976e1ced33 141 #endif
backman 7:fd976e1ced33 142
backman 7:fd976e1ced33 143
backman 7:fd976e1ced33 144
backman 7:fd976e1ced33 145
backman 7:fd976e1ced33 146
backman 8:8e49e21d80a2 147 b_r_c=(double)cam.black_centerR();
backman 8:8e49e21d80a2 148
physicsgood 10:d2401a243e8d 149 //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
physicsgood 10:d2401a243e8d 150 //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,b_r_c,PID_v);
physicsgood 10:d2401a243e8d 151 pc.printf("b_r_c %lf\r\n",b_r_c);
even 11:ffd762ae141b 152
even 11:ffd762ae141b 153 error = b_r_c - 64;
even 11:ffd762ae141b 154 integral = 0.3 * integral + error; //0.3 is the factor to make old datas become smaller
even 11:ffd762ae141b 155 derivative = error - last_error;
even 11:ffd762ae141b 156
even 11:ffd762ae141b 157 turn = error*Kp + integral*Ki + derivative*Kd;
even 11:ffd762ae141b 158
even 11:ffd762ae141b 159 if(error < 8 && error > -8){
physicsgood 10:d2401a243e8d 160 servo.set_angle(0.085);
physicsgood 10:d2401a243e8d 161 }
even 11:ffd762ae141b 162 else{
even 11:ffd762ae141b 163 servo.set_angle(0.085 + turn);
physicsgood 10:d2401a243e8d 164 }
even 11:ffd762ae141b 165
even 11:ffd762ae141b 166 leas_error = error;
backman 8:8e49e21d80a2 167
physicsgood 10:d2401a243e8d 168 //servo.set_angle(PID_v);
backman 7:fd976e1ced33 169
backman 8:8e49e21d80a2 170
backman 7:fd976e1ced33 171
backman 7:fd976e1ced33 172
backman 7:fd976e1ced33 173 #ifdef task_ma_time
backman 7:fd976e1ced33 174 task_pin=0;
backman 7:fd976e1ced33 175 #endif
backman 7:fd976e1ced33 176
backman 7:fd976e1ced33 177 }
backman 7:fd976e1ced33 178
backman 6:5a39bde2e016 179
backman 6:5a39bde2e016 180
backman 7:fd976e1ced33 181
backman 0:68c173249c01 182
backman 0:68c173249c01 183
backman 0:68c173249c01 184
backman 1:82bc25a7b68b 185 return 0;
backman 0:68c173249c01 186
backman 0:68c173249c01 187
backman 0:68c173249c01 188 }