Clark Lin / Mbed 2 deprecated 1407102

AVGG

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

main.cpp@13:e276703d5a5d, 2014-07-10 (annotated)

Committer:
physicsgood
Date:
Thu Jul 10 14:13:15 2014 +0000
Revision:
13:e276703d5a5d
Parent:
12:a3cee7619b77
Child:
14:8b17b298b4dd
AVGG YA!!!!!!!

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