123
Fork of Boboobooo by
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "servo_api.h" 00003 #include "camera_api.h" 00004 #include "motor_api.h" 00005 00006 #define Debug_cam_uart 00007 00008 00009 Serial pc(USBTX, USBRX); 00010 BX_servo servo; 00011 BX_camera cam; 00012 BX_motor MotorA('A'); 00013 BX_motor MotorB('B'); 00014 00015 //PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10); 00016 00017 00018 int main() { 00019 00020 double left = 0.025, middle = 0.038, right = 0.057; 00021 double error, turn, last_turn = middle, avg = middle; 00022 double last_error = 0; 00023 double Kp = 0.000327, Ki = 0.0001; 00024 int black_centerR, black_centerL, center, times = 0; 00025 int flag = 0, sp = 64; 00026 00027 char psudo_line[128]; 00028 00029 #ifdef Debug_cam_uart 00030 pc.baud(115200); 00031 00032 00033 while(1){ 00034 00035 cam.read(); 00036 00037 MotorA.rotate(0.0); 00038 MotorB.rotate(0.0); 00039 00040 black_centerL = cam.black_centerL(); 00041 black_centerR = cam.black_centerR(); 00042 00043 //center = (black_centerL + black_centerR) / 2; 00044 00045 /*while(1){ 00046 for(double i = 0.025; i < 0.058; i+= 0.001){ 00047 servo.set_angle(0.038); 00048 wait_ms(1000); 00049 00050 } 00051 }*/ 00052 00053 MotorA.rotate(0.15); 00054 MotorB.rotate(0.15); 00055 00056 //catch little error 00057 if(black_centerL > 64 && black_centerR > 64){ 00058 00059 center = (black_centerL <= black_centerR) ? black_centerL : black_centerR; 00060 00061 } else if(black_centerL > 64 && black_centerR < 64){ 00062 00063 center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR; 00064 00065 00066 } else if(black_centerL < 64 && black_centerR > 64){ 00067 00068 center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR; 00069 00070 } else if(black_centerL < 64 && black_centerR < 64){ 00071 00072 center = (black_centerL >= black_centerR) ? black_centerL : black_centerR; 00073 00074 } else{ 00075 00076 center = 64; 00077 } 00078 00079 00080 00081 if(black_centerL == 118 && black_centerR == 10){//no line flag = 0 00082 00083 error = 0; 00084 turn = avg; 00085 00086 flag = 0; 00087 00088 } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1 00089 00090 sp = 30; 00091 00092 if(flag == 2){ 00093 00094 sp = 64; 00095 00096 } 00097 00098 error = sp - black_centerR; 00099 turn = middle + Kp * error; 00100 00101 flag = 1; 00102 last_error = (1/3)*last_error + error; 00103 00104 } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2 00105 00106 sp = 90; 00107 00108 if(flag == 1){ 00109 00110 sp = 64; 00111 00112 } 00113 00114 error = 90 - black_centerL; 00115 turn = middle + Kp * error; 00116 00117 flag = 2; 00118 last_error = (1/3)*last_error + error; 00119 00120 } else {//flag = 3 00121 00122 if(60 < center && center < 68){ 00123 00124 turn = middle; 00125 00126 } else{ 00127 error = 64 - center; 00128 turn = Kp * error + middle; 00129 } 00130 00131 if(flag == 1){ 00132 00133 if(black_centerL < 100){ 00134 servo.set_angle(avg-0.004); 00135 turn = avg; 00136 } 00137 00138 } else if(flag == 2){ 00139 00140 if(black_centerR > 28){ 00141 servo.set_angle(avg+0.004); 00142 turn = avg; 00143 } 00144 00145 } 00146 flag = 3; 00147 } 00148 00149 last_turn += turn; 00150 times++; 00151 00152 if(times == 2){ 00153 00154 avg = last_turn / times; 00155 servo.set_angle(avg); 00156 times = last_turn = 0; 00157 00158 } 00159 00160 00161 for(int i = 128; i > 64;i--){ 00162 if(i==64) 00163 pc.printf("X"); 00164 else 00165 pc.printf("%c", cam.sign_line_imageL[i]); 00166 } 00167 00168 pc.printf(" || "); 00169 00170 for(int i = 64; i > 0; i--){ 00171 if(i==64) 00172 pc.printf("X"); 00173 else 00174 pc.printf("%c", cam.sign_line_imageR[i]); 00175 } 00176 pc.printf("\r\n"); 00177 pc.printf("black centerL: %d black_centerR: %d psudo_line: %d avg: %lf flag: %d \r\n", black_centerL, black_centerR, center, avg, flag); 00178 00179 00180 00181 //output the psudo map 00182 /*for(int i = 127; i >= 0; i--) 00183 psudo_line[i] = '0'; 00184 00185 for(int i = center-5; i < center+5; i++) 00186 psudo_line[i] = ' '; 00187 00188 for(int i = 117; i > 10; i--){ 00189 pc.printf("%c", psudo_line[i]); 00190 }*/ 00191 00192 00193 00194 00195 } 00196 00197 00198 00199 00200 00201 00202 00203 00204 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90); 00205 //-------------------------------------------- 00206 00207 00208 // servo.set_angle(( (64.0-center) /64.0 )*90 ); 00209 00210 00211 00212 00213 00214 00215 #endif 00216 00217 00218 00219 00220 00221 00222 00223 00224 00225 00226 00227 00228 00229 00230 00231 00232 return 0; 00233 00234 00235 }
Generated on Wed Jul 20 2022 19:27:32 by
1.7.2
