Clark Lin / Mbed 2 deprecated 1407102

AVGG

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

main.cpp@11:9b4788de75fe, 2014-07-10 (annotated)

Committer:
even
Date:
Thu Jul 10 06:58:37 2014 +0000
Revision:
11:9b4788de75fe
Parent:
10:e11f82b40d53
Child:
12:a3cee7619b77
7/10 finish circle round

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 9:a891053657f3 20 double left = 0.025, middle = 0.038, right = 0.054;
even 11:9b4788de75fe 21 double error, turn, last_turn = middle, avg = middle;
even 11:9b4788de75fe 22 double Kp = 0.000327, Ki = 0.025;
even 10:e11f82b40d53 23 int black_centerR, black_centerL, center, times = 0;
even 11:9b4788de75fe 24 int flag_R = 0, flag_L = 0;
even 7:f04bde0ca846 25
even 7:f04bde0ca846 26 char psudo_line[128];
backman 1:82bc25a7b68b 27
backman 3:c5f2281b3ed2 28 #ifdef Debug_cam_uart
backman 1:82bc25a7b68b 29 pc.baud(115200);
backman 1:82bc25a7b68b 30
backman 1:82bc25a7b68b 31
backman 3:c5f2281b3ed2 32 while(1){
backman 1:82bc25a7b68b 33
physicsgood 6:b046d6ff3745 34 cam.read();
even 9:a891053657f3 35
physicsgood 6:b046d6ff3745 36 MotorA.rotate(0.0);
physicsgood 6:b046d6ff3745 37 MotorB.rotate(0.0);
even 7:f04bde0ca846 38
even 7:f04bde0ca846 39 black_centerL = cam.black_centerL();
even 7:f04bde0ca846 40 black_centerR = cam.black_centerR();
even 11:9b4788de75fe 41
even 11:9b4788de75fe 42 //center = (black_centerL + black_centerR) / 2;
even 7:f04bde0ca846 43
even 11:9b4788de75fe 44 MotorA.rotate(0.15);
even 11:9b4788de75fe 45 MotorB.rotate(0.15);
even 7:f04bde0ca846 46
even 11:9b4788de75fe 47 center = 64;
even 9:a891053657f3 48
even 11:9b4788de75fe 49 if(black_centerL > 64 && black_centerR > 64){
even 11:9b4788de75fe 50
even 11:9b4788de75fe 51 center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 52
even 11:9b4788de75fe 53 } else if(black_centerL > 64 && black_centerR < 64){
even 11:9b4788de75fe 54
even 11:9b4788de75fe 55 center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 56
even 11:9b4788de75fe 57
even 11:9b4788de75fe 58 } else if(black_centerL < 64 && black_centerR > 64){
even 11:9b4788de75fe 59
even 11:9b4788de75fe 60 center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR;
even 11:9b4788de75fe 61
even 11:9b4788de75fe 62 } else if(black_centerL < 64 && black_centerR < 64){
even 11:9b4788de75fe 63
even 11:9b4788de75fe 64 center = (black_centerL >= black_centerR) ? black_centerL : black_centerR;
even 11:9b4788de75fe 65
even 11:9b4788de75fe 66 } else{
even 11:9b4788de75fe 67
even 11:9b4788de75fe 68 center = 64;
even 11:9b4788de75fe 69 }
even 10:e11f82b40d53 70
even 10:e11f82b40d53 71
even 10:e11f82b40d53 72
even 11:9b4788de75fe 73 if(black_centerL == 118 && black_centerR == 10){//no line
even 7:f04bde0ca846 74
even 9:a891053657f3 75 turn = avg;
even 7:f04bde0ca846 76
even 11:9b4788de75fe 77 } else if (black_centerL == 118 && black_centerR != 10){//no left line
even 9:a891053657f3 78
even 9:a891053657f3 79
even 11:9b4788de75fe 80 error = 8 - black_centerR;
even 11:9b4788de75fe 81 turn = Kp * error + middle;
even 11:9b4788de75fe 82 flag_L = 1;
even 7:f04bde0ca846 83
even 7:f04bde0ca846 84
even 11:9b4788de75fe 85 } else if (black_centerL != 118 && black_centerR == 10){//no right line
even 7:f04bde0ca846 86
even 11:9b4788de75fe 87 error = 120 - black_centerL;
even 11:9b4788de75fe 88 turn = Kp * error + middle;
even 11:9b4788de75fe 89 flag_R = 1;
even 7:f04bde0ca846 90
even 7:f04bde0ca846 91 } else {
even 10:e11f82b40d53 92
even 7:f04bde0ca846 93 if(60 < center && center < 68){
even 7:f04bde0ca846 94
even 7:f04bde0ca846 95 turn = middle;
even 7:f04bde0ca846 96
even 7:f04bde0ca846 97 } else{
even 7:f04bde0ca846 98 error = 64 - center;
even 7:f04bde0ca846 99 turn = Kp * error + middle;
even 7:f04bde0ca846 100 }
even 11:9b4788de75fe 101
even 11:9b4788de75fe 102 if(flag_L){
even 11:9b4788de75fe 103
even 11:9b4788de75fe 104 if(black_centerL < 100){
even 11:9b4788de75fe 105 servo.set_angle(avg-0.004);
even 11:9b4788de75fe 106 turn = avg;
even 11:9b4788de75fe 107 }
even 11:9b4788de75fe 108 flag_L = 0;
even 11:9b4788de75fe 109
even 11:9b4788de75fe 110 } else if(flag_R){
even 11:9b4788de75fe 111
even 11:9b4788de75fe 112 if(black_centerR > 28){
even 11:9b4788de75fe 113 servo.set_angle(avg+0.004);
even 11:9b4788de75fe 114 turn = avg;
even 11:9b4788de75fe 115 }
even 11:9b4788de75fe 116 flag_R = 0;
even 11:9b4788de75fe 117 }
even 7:f04bde0ca846 118 }
even 9:a891053657f3 119
even 9:a891053657f3 120 last_turn += turn;
even 9:a891053657f3 121 times++;
even 11:9b4788de75fe 122
even 11:9b4788de75fe 123 if(times == 2){
even 11:9b4788de75fe 124
even 11:9b4788de75fe 125 avg = last_turn / times;
even 11:9b4788de75fe 126 servo.set_angle(avg);
even 11:9b4788de75fe 127 times = last_turn = 0;
even 11:9b4788de75fe 128
even 7:f04bde0ca846 129 }
even 11:9b4788de75fe 130
even 11:9b4788de75fe 131
even 11:9b4788de75fe 132
even 11:9b4788de75fe 133 for(int i = 128; i > 64;i--){
backman 1:82bc25a7b68b 134 if(i==64)
backman 1:82bc25a7b68b 135 pc.printf("X");
backman 1:82bc25a7b68b 136 else
backman 2:c51647d3c14d 137 pc.printf("%c", cam.sign_line_imageL[i]);
even 11:9b4788de75fe 138 }
even 11:9b4788de75fe 139 pc.printf(" || ");
even 11:9b4788de75fe 140
even 11:9b4788de75fe 141 for(int i = 64; i > 0; i--){
even 11:9b4788de75fe 142 if(i==64)
even 11:9b4788de75fe 143 pc.printf("X");
even 11:9b4788de75fe 144 else
even 11:9b4788de75fe 145 pc.printf("%c", cam.sign_line_imageR[i]);
even 11:9b4788de75fe 146 }
even 11:9b4788de75fe 147 pc.printf("\r\n");
even 11:9b4788de75fe 148 pc.printf("black centerL: %d black_centerR: %d psudo_line: %d turn: %lf avg: %lf\r\n", black_centerL, black_centerR, center, turn, avg);
even 11:9b4788de75fe 149
even 11:9b4788de75fe 150
even 11:9b4788de75fe 151
even 11:9b4788de75fe 152 //output the psudo map
even 11:9b4788de75fe 153 /*for(int i = 127; i >= 0; i--)
even 11:9b4788de75fe 154 psudo_line[i] = '0';
even 11:9b4788de75fe 155
even 11:9b4788de75fe 156 for(int i = center-5; i < center+5; i++)
even 11:9b4788de75fe 157 psudo_line[i] = ' ';
even 11:9b4788de75fe 158
even 11:9b4788de75fe 159 for(int i = 117; i > 10; i--){
even 11:9b4788de75fe 160 pc.printf("%c", psudo_line[i]);
even 11:9b4788de75fe 161 }*/
even 11:9b4788de75fe 162
even 11:9b4788de75fe 163
even 11:9b4788de75fe 164
even 11:9b4788de75fe 165
backman 3:c5f2281b3ed2 166 }
backman 3:c5f2281b3ed2 167
even 7:f04bde0ca846 168
backman 3:c5f2281b3ed2 169
backman 3:c5f2281b3ed2 170
backman 3:c5f2281b3ed2 171
backman 3:c5f2281b3ed2 172
backman 3:c5f2281b3ed2 173
backman 3:c5f2281b3ed2 174
backman 3:c5f2281b3ed2 175 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 176 //--------------------------------------------
backman 2:c51647d3c14d 177
backman 2:c51647d3c14d 178
backman 3:c5f2281b3ed2 179 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 180
backman 3:c5f2281b3ed2 181
backman 1:82bc25a7b68b 182
backman 3:c5f2281b3ed2 183
backman 1:82bc25a7b68b 184
backman 1:82bc25a7b68b 185
backman 3:c5f2281b3ed2 186 #endif
backman 1:82bc25a7b68b 187
backman 1:82bc25a7b68b 188
backman 1:82bc25a7b68b 189
backman 1:82bc25a7b68b 190
backman 1:82bc25a7b68b 191
backman 1:82bc25a7b68b 192
backman 1:82bc25a7b68b 193
backman 1:82bc25a7b68b 194
backman 1:82bc25a7b68b 195
backman 1:82bc25a7b68b 196
backman 1:82bc25a7b68b 197
backman 1:82bc25a7b68b 198
backman 1:82bc25a7b68b 199
backman 0:68c173249c01 200
backman 0:68c173249c01 201
backman 0:68c173249c01 202
backman 1:82bc25a7b68b 203 return 0;
backman 0:68c173249c01 204
backman 0:68c173249c01 205
backman 0:68c173249c01 206 }