Clark Lin / Mbed 2 deprecated 1407102

AVGG

Dependencies:   mbed

Fork of 7_7Boboobooo 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
Child:
13:e276703d5a5d
lots of weird things

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
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 }