Clark Lin / Mbed 2 deprecated 1407102

AVGG

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

main.cpp@14:8b17b298b4dd, 2014-07-10 (annotated)

Committer:
physicsgood
Date:
Thu Jul 10 14:52:32 2014 +0000
Revision:
14:8b17b298b4dd
Parent:
13:e276703d5a5d
Child:
15:7f21c08be164
AVG

Who changed what in which revision?

UserRevisionLine numberNew 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
physicsgood 13:e276703d5a5d 20 double left = 0.027, middle = 0.038, right = 0.057;
physicsgood 13:e276703d5a5d 21 double error, turn, last_turn = 0.0, avg = middle;
physicsgood 13:e276703d5a5d 22
even 12:a3cee7619b77 23 double last_error = 0;
even 12:a3cee7619b77 24 double Kp = 0.000327, Ki = 0.0001;
even 10:e11f82b40d53 25 int black_centerR, black_centerL, center, times = 0;
even 12:a3cee7619b77 26 int flag = 0, sp = 64;
physicsgood 13:e276703d5a5d 27 int t = 0;
physicsgood 14:8b17b298b4dd 28 double last = 0.0;
physicsgood 14:8b17b298b4dd 29 double avgg = middle,last_avgg = middle, next_avgg = middle;
physicsgood 14:8b17b298b4dd 30 double v = 0.4;
even 7:f04bde0ca846 31
physicsgood 14:8b17b298b4dd 32 //char psudo_line[128];
backman 1:82bc25a7b68b 33
backman 3:c5f2281b3ed2 34 #ifdef Debug_cam_uart
backman 1:82bc25a7b68b 35 pc.baud(115200);
backman 1:82bc25a7b68b 36
backman 1:82bc25a7b68b 37
backman 3:c5f2281b3ed2 38 while(1){
even 12:a3cee7619b77 39
physicsgood 6:b046d6ff3745 40 cam.read();
even 9:a891053657f3 41
physicsgood 13:e276703d5a5d 42 //MotorA.rotate(0.3);
physicsgood 13:e276703d5a5d 43 //MotorB.rotate(0.3);
even 7:f04bde0ca846 44
even 7:f04bde0ca846 45 black_centerL = cam.black_centerL();
even 7:f04bde0ca846 46 black_centerR = cam.black_centerR();
even 11:9b4788de75fe 47
even 11:9b4788de75fe 48 //center = (black_centerL + black_centerR) / 2;
even 7:f04bde0ca846 49
even 12:a3cee7619b77 50 /*while(1){
even 12:a3cee7619b77 51 for(double i = 0.025; i < 0.058; i+= 0.001){
even 12:a3cee7619b77 52 servo.set_angle(0.038);
even 12:a3cee7619b77 53 wait_ms(1000);
even 12:a3cee7619b77 54
even 12:a3cee7619b77 55 }
even 12:a3cee7619b77 56 }*/
even 12:a3cee7619b77 57
physicsgood 13:e276703d5a5d 58
even 7:f04bde0ca846 59
even 12:a3cee7619b77 60 //catch little error
even 11:9b4788de75fe 61 if(black_centerL > 64 && black_centerR > 64){
even 11:9b4788de75fe 62
even 11:9b4788de75fe 63 center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 64
even 11:9b4788de75fe 65 } else if(black_centerL > 64 && black_centerR < 64){
even 11:9b4788de75fe 66
even 11:9b4788de75fe 67 center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 68
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 = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR;
even 11:9b4788de75fe 73
even 11:9b4788de75fe 74 } else if(black_centerL < 64 && black_centerR < 64){
even 11:9b4788de75fe 75
even 11:9b4788de75fe 76 center = (black_centerL >= black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 77
even 11:9b4788de75fe 78 } else{
even 11:9b4788de75fe 79
even 11:9b4788de75fe 80 center = 64;
even 11:9b4788de75fe 81 }
even 10:e11f82b40d53 82
even 10:e11f82b40d53 83
even 10:e11f82b40d53 84
even 12:a3cee7619b77 85 if(black_centerL == 118 && black_centerR == 10){//no line flag = 0
even 7:f04bde0ca846 86
even 12:a3cee7619b77 87 error = 0;
physicsgood 13:e276703d5a5d 88 //turn = avg;
physicsgood 13:e276703d5a5d 89 if(avgg < 0.038)
physicsgood 13:e276703d5a5d 90 turn = 0.026;
physicsgood 13:e276703d5a5d 91 else
physicsgood 13:e276703d5a5d 92 turn = 0.054;
physicsgood 13:e276703d5a5d 93 /*else
physicsgood 13:e276703d5a5d 94 turn = middle;*/
even 12:a3cee7619b77 95 flag = 0;
even 7:f04bde0ca846 96
even 12:a3cee7619b77 97 } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1
even 12:a3cee7619b77 98
even 12:a3cee7619b77 99 sp = 30;
even 12:a3cee7619b77 100 if(flag == 2){
even 12:a3cee7619b77 101
even 12:a3cee7619b77 102 sp = 64;
even 12:a3cee7619b77 103
even 12:a3cee7619b77 104 }
even 12:a3cee7619b77 105
even 12:a3cee7619b77 106 error = sp - black_centerR;
even 12:a3cee7619b77 107 turn = middle + Kp * error;
even 12:a3cee7619b77 108 flag = 1;
even 12:a3cee7619b77 109 last_error = (1/3)*last_error + error;
even 7:f04bde0ca846 110
even 12:a3cee7619b77 111 } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2
even 7:f04bde0ca846 112
even 12:a3cee7619b77 113 sp = 90;
even 12:a3cee7619b77 114
even 12:a3cee7619b77 115 if(flag == 1){
even 12:a3cee7619b77 116
even 12:a3cee7619b77 117 sp = 64;
even 12:a3cee7619b77 118
even 12:a3cee7619b77 119 }
even 12:a3cee7619b77 120
even 12:a3cee7619b77 121 error = 90 - black_centerL;
even 12:a3cee7619b77 122 turn = middle + Kp * error;
even 7:f04bde0ca846 123
even 12:a3cee7619b77 124 flag = 2;
even 12:a3cee7619b77 125 last_error = (1/3)*last_error + error;
even 12:a3cee7619b77 126
even 12:a3cee7619b77 127 } else {//flag = 3
even 10:e11f82b40d53 128
even 7:f04bde0ca846 129 if(60 < center && center < 68){
even 7:f04bde0ca846 130
even 7:f04bde0ca846 131 turn = middle;
even 7:f04bde0ca846 132
even 7:f04bde0ca846 133 } else{
even 7:f04bde0ca846 134 error = 64 - center;
even 7:f04bde0ca846 135 turn = Kp * error + middle;
even 7:f04bde0ca846 136 }
even 11:9b4788de75fe 137
even 12:a3cee7619b77 138 if(flag == 1){
even 11:9b4788de75fe 139
even 11:9b4788de75fe 140 if(black_centerL < 100){
even 11:9b4788de75fe 141 servo.set_angle(avg-0.004);
even 11:9b4788de75fe 142 turn = avg;
even 11:9b4788de75fe 143 }
even 11:9b4788de75fe 144
even 12:a3cee7619b77 145 } else if(flag == 2){
even 11:9b4788de75fe 146
even 11:9b4788de75fe 147 if(black_centerR > 28){
even 11:9b4788de75fe 148 servo.set_angle(avg+0.004);
even 11:9b4788de75fe 149 turn = avg;
even 11:9b4788de75fe 150 }
even 12:a3cee7619b77 151
even 11:9b4788de75fe 152 }
even 12:a3cee7619b77 153 flag = 3;
even 7:f04bde0ca846 154 }
even 9:a891053657f3 155
even 9:a891053657f3 156 last_turn += turn;
physicsgood 13:e276703d5a5d 157 last += turn;
even 9:a891053657f3 158 times++;
physicsgood 13:e276703d5a5d 159 t++;
even 11:9b4788de75fe 160
even 11:9b4788de75fe 161 if(times == 2){
even 11:9b4788de75fe 162
even 11:9b4788de75fe 163 avg = last_turn / times;
physicsgood 13:e276703d5a5d 164 if(avg < 0.042 && avg > 0.034){
physicsgood 14:8b17b298b4dd 165 v = 0.4;
physicsgood 13:e276703d5a5d 166 }
physicsgood 13:e276703d5a5d 167 else{
physicsgood 14:8b17b298b4dd 168 v = 0.18;
physicsgood 13:e276703d5a5d 169 }
physicsgood 14:8b17b298b4dd 170 MotorA.rotate(v);
physicsgood 14:8b17b298b4dd 171 MotorB.rotate(v);
physicsgood 14:8b17b298b4dd 172 servo.set_angle((avg*7 + next_avgg)/8);
physicsgood 14:8b17b298b4dd 173 //servo.set_angle(avg);
even 11:9b4788de75fe 174 times = last_turn = 0;
even 11:9b4788de75fe 175
even 7:f04bde0ca846 176 }
physicsgood 14:8b17b298b4dd 177 if(t == 500){
physicsgood 14:8b17b298b4dd 178 last_avgg = avgg;
physicsgood 13:e276703d5a5d 179 avgg = last / t;
physicsgood 14:8b17b298b4dd 180 next_avgg = 2*avgg -last_avgg;
physicsgood 13:e276703d5a5d 181 t = 0;
physicsgood 13:e276703d5a5d 182 last = 0.0;
physicsgood 13:e276703d5a5d 183 }
even 11:9b4788de75fe 184
physicsgood 13:e276703d5a5d 185 /*for(int i = 128; i > 64;i--){
even 12:a3cee7619b77 186 if(i==64)
even 12:a3cee7619b77 187 pc.printf("X");
even 12:a3cee7619b77 188 else
even 12:a3cee7619b77 189 pc.printf("%c", cam.sign_line_imageL[i]);
even 12:a3cee7619b77 190 }
even 12:a3cee7619b77 191
even 12:a3cee7619b77 192 pc.printf(" || ");
even 11:9b4788de75fe 193
even 12:a3cee7619b77 194 for(int i = 64; i > 0; i--){
even 12:a3cee7619b77 195 if(i==64)
even 12:a3cee7619b77 196 pc.printf("X");
even 12:a3cee7619b77 197 else
even 12:a3cee7619b77 198 pc.printf("%c", cam.sign_line_imageR[i]);
physicsgood 13:e276703d5a5d 199 }*/
even 12:a3cee7619b77 200 pc.printf("\r\n");
physicsgood 13:e276703d5a5d 201 pc.printf("black centerL: %d black_centerR: %d psudo_line: %d avg: %lf flag: %d avgg: %lf\r\n", black_centerL, black_centerR, center, avg, flag,avgg);
even 11:9b4788de75fe 202
even 11:9b4788de75fe 203
even 11:9b4788de75fe 204
even 11:9b4788de75fe 205 //output the psudo map
even 11:9b4788de75fe 206 /*for(int i = 127; i >= 0; i--)
even 11:9b4788de75fe 207 psudo_line[i] = '0';
even 11:9b4788de75fe 208
even 11:9b4788de75fe 209 for(int i = center-5; i < center+5; i++)
even 11:9b4788de75fe 210 psudo_line[i] = ' ';
even 11:9b4788de75fe 211
even 11:9b4788de75fe 212 for(int i = 117; i > 10; i--){
even 11:9b4788de75fe 213 pc.printf("%c", psudo_line[i]);
even 11:9b4788de75fe 214 }*/
even 11:9b4788de75fe 215
even 11:9b4788de75fe 216
even 11:9b4788de75fe 217
even 11:9b4788de75fe 218
backman 3:c5f2281b3ed2 219 }
backman 3:c5f2281b3ed2 220
even 7:f04bde0ca846 221
backman 3:c5f2281b3ed2 222
backman 3:c5f2281b3ed2 223
backman 3:c5f2281b3ed2 224
backman 3:c5f2281b3ed2 225
backman 3:c5f2281b3ed2 226
backman 3:c5f2281b3ed2 227
backman 3:c5f2281b3ed2 228 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 229 //--------------------------------------------
backman 2:c51647d3c14d 230
backman 2:c51647d3c14d 231
backman 3:c5f2281b3ed2 232 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 233
backman 3:c5f2281b3ed2 234
backman 1:82bc25a7b68b 235
backman 3:c5f2281b3ed2 236
backman 1:82bc25a7b68b 237
backman 1:82bc25a7b68b 238
backman 3:c5f2281b3ed2 239 #endif
backman 1:82bc25a7b68b 240
backman 1:82bc25a7b68b 241
backman 1:82bc25a7b68b 242
backman 1:82bc25a7b68b 243
backman 1:82bc25a7b68b 244
backman 1:82bc25a7b68b 245
backman 1:82bc25a7b68b 246
backman 1:82bc25a7b68b 247
backman 1:82bc25a7b68b 248
backman 1:82bc25a7b68b 249
backman 1:82bc25a7b68b 250
backman 1:82bc25a7b68b 251
backman 1:82bc25a7b68b 252
backman 0:68c173249c01 253
backman 0:68c173249c01 254
backman 0:68c173249c01 255
backman 1:82bc25a7b68b 256 return 0;
backman 0:68c173249c01 257
backman 0:68c173249c01 258
backman 0:68c173249c01 259 }