譯文 張
/
7_7Boboobooo
123
Fork of Boboobooo by
main.cpp@12:a3cee7619b77, 2014-07-10 (annotated)
- Committer:
- even
- Date:
- Thu Jul 10 10:22:14 2014 +0000
- Revision:
- 12:a3cee7619b77
- Parent:
- 11:9b4788de75fe
lots of weird things
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
backman | 0:68c173249c01 | 1 | #include "mbed.h" |
backman | 1:82bc25a7b68b | 2 | #include "servo_api.h" |
backman | 1:82bc25a7b68b | 3 | #include "camera_api.h" |
even | 7:f04bde0ca846 | 4 | #include "motor_api.h" |
backman | 3:c5f2281b3ed2 | 5 | |
backman | 3:c5f2281b3ed2 | 6 | #define Debug_cam_uart |
backman | 0:68c173249c01 | 7 | |
even | 9:a891053657f3 | 8 | |
backman | 1:82bc25a7b68b | 9 | Serial pc(USBTX, USBRX); |
backman | 1:82bc25a7b68b | 10 | BX_servo servo; |
physicsgood | 6:b046d6ff3745 | 11 | BX_camera cam; |
physicsgood | 6:b046d6ff3745 | 12 | BX_motor MotorA('A'); |
physicsgood | 6:b046d6ff3745 | 13 | BX_motor MotorB('B'); |
backman | 0:68c173249c01 | 14 | |
even | 7:f04bde0ca846 | 15 | //PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10); |
backman | 3:c5f2281b3ed2 | 16 | |
backman | 3:c5f2281b3ed2 | 17 | |
backman | 0:68c173249c01 | 18 | int main() { |
even | 9:a891053657f3 | 19 | |
even | 12:a3cee7619b77 | 20 | double left = 0.025, middle = 0.038, right = 0.057; |
even | 11:9b4788de75fe | 21 | double error, turn, last_turn = middle, avg = middle; |
even | 12:a3cee7619b77 | 22 | double last_error = 0; |
even | 12:a3cee7619b77 | 23 | double Kp = 0.000327, Ki = 0.0001; |
even | 10:e11f82b40d53 | 24 | int black_centerR, black_centerL, center, times = 0; |
even | 12:a3cee7619b77 | 25 | int flag = 0, sp = 64; |
even | 7:f04bde0ca846 | 26 | |
even | 7:f04bde0ca846 | 27 | char psudo_line[128]; |
backman | 1:82bc25a7b68b | 28 | |
backman | 3:c5f2281b3ed2 | 29 | #ifdef Debug_cam_uart |
backman | 1:82bc25a7b68b | 30 | pc.baud(115200); |
backman | 1:82bc25a7b68b | 31 | |
backman | 1:82bc25a7b68b | 32 | |
backman | 3:c5f2281b3ed2 | 33 | while(1){ |
even | 12:a3cee7619b77 | 34 | |
physicsgood | 6:b046d6ff3745 | 35 | cam.read(); |
even | 9:a891053657f3 | 36 | |
physicsgood | 6:b046d6ff3745 | 37 | MotorA.rotate(0.0); |
physicsgood | 6:b046d6ff3745 | 38 | MotorB.rotate(0.0); |
even | 7:f04bde0ca846 | 39 | |
even | 7:f04bde0ca846 | 40 | black_centerL = cam.black_centerL(); |
even | 7:f04bde0ca846 | 41 | black_centerR = cam.black_centerR(); |
even | 11:9b4788de75fe | 42 | |
even | 11:9b4788de75fe | 43 | //center = (black_centerL + black_centerR) / 2; |
even | 7:f04bde0ca846 | 44 | |
even | 12:a3cee7619b77 | 45 | /*while(1){ |
even | 12:a3cee7619b77 | 46 | for(double i = 0.025; i < 0.058; i+= 0.001){ |
even | 12:a3cee7619b77 | 47 | servo.set_angle(0.038); |
even | 12:a3cee7619b77 | 48 | wait_ms(1000); |
even | 12:a3cee7619b77 | 49 | |
even | 12:a3cee7619b77 | 50 | } |
even | 12:a3cee7619b77 | 51 | }*/ |
even | 12:a3cee7619b77 | 52 | |
even | 11:9b4788de75fe | 53 | MotorA.rotate(0.15); |
even | 11:9b4788de75fe | 54 | MotorB.rotate(0.15); |
even | 7:f04bde0ca846 | 55 | |
even | 12:a3cee7619b77 | 56 | //catch little error |
even | 11:9b4788de75fe | 57 | if(black_centerL > 64 && black_centerR > 64){ |
even | 11:9b4788de75fe | 58 | |
even | 11:9b4788de75fe | 59 | center = (black_centerL <= black_centerR) ? black_centerL : black_centerR; |
even | 11:9b4788de75fe | 60 | |
even | 11:9b4788de75fe | 61 | } else if(black_centerL > 64 && black_centerR < 64){ |
even | 11:9b4788de75fe | 62 | |
even | 11:9b4788de75fe | 63 | center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR; |
even | 11:9b4788de75fe | 64 | |
even | 11:9b4788de75fe | 65 | |
even | 11:9b4788de75fe | 66 | } else if(black_centerL < 64 && black_centerR > 64){ |
even | 11:9b4788de75fe | 67 | |
even | 11:9b4788de75fe | 68 | center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR; |
even | 11:9b4788de75fe | 69 | |
even | 11:9b4788de75fe | 70 | } else if(black_centerL < 64 && black_centerR < 64){ |
even | 11:9b4788de75fe | 71 | |
even | 11:9b4788de75fe | 72 | center = (black_centerL >= black_centerR) ? black_centerL : black_centerR; |
even | 11:9b4788de75fe | 73 | |
even | 11:9b4788de75fe | 74 | } else{ |
even | 11:9b4788de75fe | 75 | |
even | 11:9b4788de75fe | 76 | center = 64; |
even | 11:9b4788de75fe | 77 | } |
even | 10:e11f82b40d53 | 78 | |
even | 10:e11f82b40d53 | 79 | |
even | 10:e11f82b40d53 | 80 | |
even | 12:a3cee7619b77 | 81 | if(black_centerL == 118 && black_centerR == 10){//no line flag = 0 |
even | 7:f04bde0ca846 | 82 | |
even | 12:a3cee7619b77 | 83 | error = 0; |
even | 9:a891053657f3 | 84 | turn = avg; |
even | 12:a3cee7619b77 | 85 | |
even | 12:a3cee7619b77 | 86 | flag = 0; |
even | 7:f04bde0ca846 | 87 | |
even | 12:a3cee7619b77 | 88 | } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1 |
even | 12:a3cee7619b77 | 89 | |
even | 12:a3cee7619b77 | 90 | sp = 30; |
even | 9:a891053657f3 | 91 | |
even | 12:a3cee7619b77 | 92 | if(flag == 2){ |
even | 12:a3cee7619b77 | 93 | |
even | 12:a3cee7619b77 | 94 | sp = 64; |
even | 12:a3cee7619b77 | 95 | |
even | 12:a3cee7619b77 | 96 | } |
even | 12:a3cee7619b77 | 97 | |
even | 12:a3cee7619b77 | 98 | error = sp - black_centerR; |
even | 12:a3cee7619b77 | 99 | turn = middle + Kp * error; |
even | 9:a891053657f3 | 100 | |
even | 12:a3cee7619b77 | 101 | flag = 1; |
even | 12:a3cee7619b77 | 102 | last_error = (1/3)*last_error + error; |
even | 7:f04bde0ca846 | 103 | |
even | 12:a3cee7619b77 | 104 | } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2 |
even | 7:f04bde0ca846 | 105 | |
even | 12:a3cee7619b77 | 106 | sp = 90; |
even | 12:a3cee7619b77 | 107 | |
even | 12:a3cee7619b77 | 108 | if(flag == 1){ |
even | 12:a3cee7619b77 | 109 | |
even | 12:a3cee7619b77 | 110 | sp = 64; |
even | 12:a3cee7619b77 | 111 | |
even | 12:a3cee7619b77 | 112 | } |
even | 12:a3cee7619b77 | 113 | |
even | 12:a3cee7619b77 | 114 | error = 90 - black_centerL; |
even | 12:a3cee7619b77 | 115 | turn = middle + Kp * error; |
even | 7:f04bde0ca846 | 116 | |
even | 12:a3cee7619b77 | 117 | flag = 2; |
even | 12:a3cee7619b77 | 118 | last_error = (1/3)*last_error + error; |
even | 12:a3cee7619b77 | 119 | |
even | 12:a3cee7619b77 | 120 | } else {//flag = 3 |
even | 10:e11f82b40d53 | 121 | |
even | 7:f04bde0ca846 | 122 | if(60 < center && center < 68){ |
even | 7:f04bde0ca846 | 123 | |
even | 7:f04bde0ca846 | 124 | turn = middle; |
even | 7:f04bde0ca846 | 125 | |
even | 7:f04bde0ca846 | 126 | } else{ |
even | 7:f04bde0ca846 | 127 | error = 64 - center; |
even | 7:f04bde0ca846 | 128 | turn = Kp * error + middle; |
even | 7:f04bde0ca846 | 129 | } |
even | 11:9b4788de75fe | 130 | |
even | 12:a3cee7619b77 | 131 | if(flag == 1){ |
even | 11:9b4788de75fe | 132 | |
even | 11:9b4788de75fe | 133 | if(black_centerL < 100){ |
even | 11:9b4788de75fe | 134 | servo.set_angle(avg-0.004); |
even | 11:9b4788de75fe | 135 | turn = avg; |
even | 11:9b4788de75fe | 136 | } |
even | 11:9b4788de75fe | 137 | |
even | 12:a3cee7619b77 | 138 | } else if(flag == 2){ |
even | 11:9b4788de75fe | 139 | |
even | 11:9b4788de75fe | 140 | if(black_centerR > 28){ |
even | 11:9b4788de75fe | 141 | servo.set_angle(avg+0.004); |
even | 11:9b4788de75fe | 142 | turn = avg; |
even | 11:9b4788de75fe | 143 | } |
even | 12:a3cee7619b77 | 144 | |
even | 11:9b4788de75fe | 145 | } |
even | 12:a3cee7619b77 | 146 | flag = 3; |
even | 7:f04bde0ca846 | 147 | } |
even | 9:a891053657f3 | 148 | |
even | 9:a891053657f3 | 149 | last_turn += turn; |
even | 9:a891053657f3 | 150 | times++; |
even | 11:9b4788de75fe | 151 | |
even | 11:9b4788de75fe | 152 | if(times == 2){ |
even | 11:9b4788de75fe | 153 | |
even | 11:9b4788de75fe | 154 | avg = last_turn / times; |
even | 11:9b4788de75fe | 155 | servo.set_angle(avg); |
even | 11:9b4788de75fe | 156 | times = last_turn = 0; |
even | 11:9b4788de75fe | 157 | |
even | 7:f04bde0ca846 | 158 | } |
even | 11:9b4788de75fe | 159 | |
even | 11:9b4788de75fe | 160 | |
even | 12:a3cee7619b77 | 161 | for(int i = 128; i > 64;i--){ |
even | 12:a3cee7619b77 | 162 | if(i==64) |
even | 12:a3cee7619b77 | 163 | pc.printf("X"); |
even | 12:a3cee7619b77 | 164 | else |
even | 12:a3cee7619b77 | 165 | pc.printf("%c", cam.sign_line_imageL[i]); |
even | 12:a3cee7619b77 | 166 | } |
even | 12:a3cee7619b77 | 167 | |
even | 12:a3cee7619b77 | 168 | pc.printf(" || "); |
even | 11:9b4788de75fe | 169 | |
even | 12:a3cee7619b77 | 170 | for(int i = 64; i > 0; i--){ |
even | 12:a3cee7619b77 | 171 | if(i==64) |
even | 12:a3cee7619b77 | 172 | pc.printf("X"); |
even | 12:a3cee7619b77 | 173 | else |
even | 12:a3cee7619b77 | 174 | pc.printf("%c", cam.sign_line_imageR[i]); |
even | 12:a3cee7619b77 | 175 | } |
even | 12:a3cee7619b77 | 176 | pc.printf("\r\n"); |
even | 12:a3cee7619b77 | 177 | pc.printf("black centerL: %d black_centerR: %d psudo_line: %d avg: %lf flag: %d \r\n", black_centerL, black_centerR, center, avg, flag); |
even | 11:9b4788de75fe | 178 | |
even | 11:9b4788de75fe | 179 | |
even | 11:9b4788de75fe | 180 | |
even | 11:9b4788de75fe | 181 | //output the psudo map |
even | 11:9b4788de75fe | 182 | /*for(int i = 127; i >= 0; i--) |
even | 11:9b4788de75fe | 183 | psudo_line[i] = '0'; |
even | 11:9b4788de75fe | 184 | |
even | 11:9b4788de75fe | 185 | for(int i = center-5; i < center+5; i++) |
even | 11:9b4788de75fe | 186 | psudo_line[i] = ' '; |
even | 11:9b4788de75fe | 187 | |
even | 11:9b4788de75fe | 188 | for(int i = 117; i > 10; i--){ |
even | 11:9b4788de75fe | 189 | pc.printf("%c", psudo_line[i]); |
even | 11:9b4788de75fe | 190 | }*/ |
even | 11:9b4788de75fe | 191 | |
even | 11:9b4788de75fe | 192 | |
even | 11:9b4788de75fe | 193 | |
even | 11:9b4788de75fe | 194 | |
backman | 3:c5f2281b3ed2 | 195 | } |
backman | 3:c5f2281b3ed2 | 196 | |
even | 7:f04bde0ca846 | 197 | |
backman | 3:c5f2281b3ed2 | 198 | |
backman | 3:c5f2281b3ed2 | 199 | |
backman | 3:c5f2281b3ed2 | 200 | |
backman | 3:c5f2281b3ed2 | 201 | |
backman | 3:c5f2281b3ed2 | 202 | |
backman | 3:c5f2281b3ed2 | 203 | |
backman | 3:c5f2281b3ed2 | 204 | // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90); |
backman | 1:82bc25a7b68b | 205 | //-------------------------------------------- |
backman | 2:c51647d3c14d | 206 | |
backman | 2:c51647d3c14d | 207 | |
backman | 3:c5f2281b3ed2 | 208 | // servo.set_angle(( (64.0-center) /64.0 )*90 ); |
backman | 2:c51647d3c14d | 209 | |
backman | 3:c5f2281b3ed2 | 210 | |
backman | 1:82bc25a7b68b | 211 | |
backman | 3:c5f2281b3ed2 | 212 | |
backman | 1:82bc25a7b68b | 213 | |
backman | 1:82bc25a7b68b | 214 | |
backman | 3:c5f2281b3ed2 | 215 | #endif |
backman | 1:82bc25a7b68b | 216 | |
backman | 1:82bc25a7b68b | 217 | |
backman | 1:82bc25a7b68b | 218 | |
backman | 1:82bc25a7b68b | 219 | |
backman | 1:82bc25a7b68b | 220 | |
backman | 1:82bc25a7b68b | 221 | |
backman | 1:82bc25a7b68b | 222 | |
backman | 1:82bc25a7b68b | 223 | |
backman | 1:82bc25a7b68b | 224 | |
backman | 1:82bc25a7b68b | 225 | |
backman | 1:82bc25a7b68b | 226 | |
backman | 1:82bc25a7b68b | 227 | |
backman | 1:82bc25a7b68b | 228 | |
backman | 0:68c173249c01 | 229 | |
backman | 0:68c173249c01 | 230 | |
backman | 0:68c173249c01 | 231 | |
backman | 1:82bc25a7b68b | 232 | return 0; |
backman | 0:68c173249c01 | 233 | |
backman | 0:68c173249c01 | 234 | |
backman | 0:68c173249c01 | 235 | } |