Clark Lin / Mbed 2 deprecated BX-car

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.

Dependencies:   mbed

Fork of BX-car by kao yi

main.cpp@12:f7acda4545ba, 2014-06-30 (annotated)

Committer:
physicsgood
Date:
Mon Jun 30 02:07:57 2014 +0000
Revision:
12:f7acda4545ba
Parent:
11:6189b2fc618a
Child:
13:0f0d341d6d61
QQQ

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
physicsgood 12:f7acda4545ba 29 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,0.00125,0.0,0.0);
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);
physicsgood 12:f7acda4545ba 41
backman 9:33b99cb45e99 42 while(1){
physicsgood 12:f7acda4545ba 43 if(tsi.readPercentage()>0.00011)
physicsgood 12:f7acda4545ba 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;
physicsgood 12:f7acda4545ba 53
physicsgood 12:f7acda4545ba 54 //double Kp = 0.00078, Ki = 0.0000, Kd = 0.0000;
physicsgood 12:f7acda4545ba 55 double integral = 0, derivative = 0, error = 0, last_error = 0, turn = 0;
physicsgood 12:f7acda4545ba 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 12:f7acda4545ba 149 PID_v=cam_to_M_ctrlr.compute(200,b_r_c,20.0);//set_angle()
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 12:f7acda4545ba 151 //b_r_c range 0~128
physicsgood 12:f7acda4545ba 152 /*error = 20 - b_r_c;
physicsgood 12:f7acda4545ba 153 integral = 0.3 * integral + error; //0.3 is the factor to make old datas become smaller
physicsgood 12:f7acda4545ba 154 derivative = error - last_error;
physicsgood 12:f7acda4545ba 155
physicsgood 12:f7acda4545ba 156 //turn = error*Kp + integral*Ki + derivative*Kd;
physicsgood 12:f7acda4545ba 157
physicsgood 12:f7acda4545ba 158 if(error < -8 && error >8){
physicsgood 10:d2401a243e8d 159 servo.set_angle(0.085);
physicsgood 10:d2401a243e8d 160 }
physicsgood 12:f7acda4545ba 161 else if (error > 8 && error <108){
physicsgood 12:f7acda4545ba 162 servo.set_angle(0.085+turn);
physicsgood 10:d2401a243e8d 163 }
physicsgood 12:f7acda4545ba 164 else if (error <-8 && error>-20){
physicsgood 12:f7acda4545ba 165 servo.set_angle(0.085+turn);
physicsgood 12:f7acda4545ba 166 }*/
physicsgood 12:f7acda4545ba 167
physicsgood 12:f7acda4545ba 168 last_error = error;
backman 8:8e49e21d80a2 169
physicsgood 12:f7acda4545ba 170 servo.set_angle(PID_v);
backman 7:fd976e1ced33 171
backman 8:8e49e21d80a2 172
backman 7:fd976e1ced33 173
backman 7:fd976e1ced33 174
backman 7:fd976e1ced33 175 #ifdef task_ma_time
backman 7:fd976e1ced33 176 task_pin=0;
backman 7:fd976e1ced33 177 #endif
backman 7:fd976e1ced33 178
backman 7:fd976e1ced33 179 }
backman 7:fd976e1ced33 180
backman 6:5a39bde2e016 181
backman 6:5a39bde2e016 182
backman 7:fd976e1ced33 183
backman 0:68c173249c01 184
backman 0:68c173249c01 185
backman 0:68c173249c01 186
backman 1:82bc25a7b68b 187 return 0;
backman 0:68c173249c01 188
backman 0:68c173249c01 189
backman 0:68c173249c01 190 }