譯文 張 / Mbed 2 deprecated 7_7Boboobooo

123

Dependencies:   mbed

Fork of Boboobooo by Clark Lin

main.cpp@9:a891053657f3, 2014-07-09 (annotated)

Committer:
even
Date:
Wed Jul 09 14:08:11 2014 +0000
Revision:
9:a891053657f3
Parent:
8:c2c3aee85c2d
Child:
10:e11f82b40d53
7/9 psudo I

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
even 7:f04bde0ca846 2 //#include "controller.h"
backman 1:82bc25a7b68b 3 #include "servo_api.h"
backman 1:82bc25a7b68b 4 #include "camera_api.h"
even 7:f04bde0ca846 5 #include "motor_api.h"
backman 3:c5f2281b3ed2 6
backman 3:c5f2281b3ed2 7 #define Debug_cam_uart
backman 0:68c173249c01 8
even 9:a891053657f3 9
backman 1:82bc25a7b68b 10 Serial pc(USBTX, USBRX);
backman 1:82bc25a7b68b 11 BX_servo servo;
physicsgood 6:b046d6ff3745 12 BX_camera cam;
physicsgood 6:b046d6ff3745 13 BX_motor MotorA('A');
physicsgood 6:b046d6ff3745 14 BX_motor MotorB('B');
backman 0:68c173249c01 15
even 7:f04bde0ca846 16 //PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10);
backman 3:c5f2281b3ed2 17
backman 3:c5f2281b3ed2 18
backman 0:68c173249c01 19 int main() {
even 9:a891053657f3 20
even 9:a891053657f3 21 double left = 0.025, middle = 0.038, right = 0.054;
even 9:a891053657f3 22 double error, turn, last_turn = middle, avg = 0;
even 9:a891053657f3 23 int times = 0;
even 9:a891053657f3 24 double Kp = 0.000527, Ki = 0.025;
even 7:f04bde0ca846 25 int black_centerR, black_centerL, center;
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){
backman 1:82bc25a7b68b 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 7:f04bde0ca846 42 center = (black_centerL + black_centerR) / 2;
even 7:f04bde0ca846 43
even 7:f04bde0ca846 44 MotorA.rotate(0.2);
even 7:f04bde0ca846 45 MotorB.rotate(0.2);
even 7:f04bde0ca846 46
even 9:a891053657f3 47
even 9:a891053657f3 48 if(times == 10){
even 9:a891053657f3 49 avg = last_turn / 10;
even 9:a891053657f3 50 last_turn = times = 0;
even 9:a891053657f3 51 }
even 9:a891053657f3 52
even 7:f04bde0ca846 53 if(black_centerL == 128 && black_centerR == 0){//no line
even 7:f04bde0ca846 54
even 9:a891053657f3 55 turn = avg;
even 7:f04bde0ca846 56
even 7:f04bde0ca846 57 } else if (black_centerL == 128 && black_centerR != 0){//no left line
even 9:a891053657f3 58
even 9:a891053657f3 59
even 9:a891053657f3 60 /*error = 64 - black_centerR;
even 9:a891053657f3 61 turn = Kp * error + middle;*/
even 9:a891053657f3 62 turn = avg;
even 9:a891053657f3 63
even 7:f04bde0ca846 64
even 7:f04bde0ca846 65
even 7:f04bde0ca846 66 } else if (black_centerL != 128 && black_centerR == 0){//no right line
even 7:f04bde0ca846 67
even 9:a891053657f3 68
even 9:a891053657f3 69 /*error = 64 - black_centerL;
even 9:a891053657f3 70 turn = Kp * error + middle;*/
even 9:a891053657f3 71 turn = avg;
even 7:f04bde0ca846 72
even 7:f04bde0ca846 73 } else {
even 7:f04bde0ca846 74 if(60 < center && center < 68){
even 7:f04bde0ca846 75
even 7:f04bde0ca846 76 turn = middle;
even 7:f04bde0ca846 77
even 7:f04bde0ca846 78 } else{
even 7:f04bde0ca846 79 error = 64 - center;
even 7:f04bde0ca846 80 turn = Kp * error + middle;
even 7:f04bde0ca846 81 }
even 7:f04bde0ca846 82 }
even 9:a891053657f3 83
even 9:a891053657f3 84 /*if(times == 10){
even 9:a891053657f3 85 pc.printf(">>>>>>>>>>>>>>>\r\n\r\n\r\n\r\n\r\n\r\n\r\n");
even 9:a891053657f3 86 avg /= 10;
even 9:a891053657f3 87
even 9:a891053657f3 88 if(avg > 0.045){
even 9:a891053657f3 89 turn += Ki * avg;
even 9:a891053657f3 90 } else if(avg < 0.037){
even 9:a891053657f3 91 turn -= Ki * avg;
even 7:f04bde0ca846 92 }
even 9:a891053657f3 93
even 9:a891053657f3 94 avg = times = 0;
even 9:a891053657f3 95 }*/
even 9:a891053657f3 96
even 9:a891053657f3 97 last_turn += turn;
even 9:a891053657f3 98 times++;
even 7:f04bde0ca846 99 servo.set_angle(turn);
even 7:f04bde0ca846 100
even 7:f04bde0ca846 101
even 7:f04bde0ca846 102 //output the psudo map
even 7:f04bde0ca846 103 for(int i = 127; i >= 0; i--)
even 7:f04bde0ca846 104 psudo_line[i] = '0';
even 7:f04bde0ca846 105
even 7:f04bde0ca846 106 for(int i = center-5; i < center+5; i++)
even 7:f04bde0ca846 107 psudo_line[i] = ' ';
even 7:f04bde0ca846 108
even 7:f04bde0ca846 109 for(int i = 117; i > 10; i--){
even 7:f04bde0ca846 110 pc.printf("%c", psudo_line[i]);
even 7:f04bde0ca846 111 }
even 7:f04bde0ca846 112 pc.printf("\r\n");
even 9:a891053657f3 113 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 7:f04bde0ca846 114
even 7:f04bde0ca846 115
even 7:f04bde0ca846 116 /*for(int i = 128; i > 64;i--){
backman 1:82bc25a7b68b 117 if(i==64)
backman 1:82bc25a7b68b 118 pc.printf("X");
backman 1:82bc25a7b68b 119 else
backman 2:c51647d3c14d 120 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 121 }
backman 2:c51647d3c14d 122 pc.printf(" || ");
backman 2:c51647d3c14d 123
even 7:f04bde0ca846 124 for(int i = 64; i > 0; i--){
backman 2:c51647d3c14d 125 if(i==64)
backman 2:c51647d3c14d 126 pc.printf("X");
backman 2:c51647d3c14d 127 else
backman 2:c51647d3c14d 128 pc.printf("%c", cam.sign_line_imageR[i]);
backman 1:82bc25a7b68b 129 }
even 7:f04bde0ca846 130 pc.printf("\r\n");
even 7:f04bde0ca846 131 pc.printf("black_centerL: %d black_centerR: %d\r\n", black_centerL, black_centerR);
even 7:f04bde0ca846 132 */
backman 3:c5f2281b3ed2 133 }
backman 3:c5f2281b3ed2 134
even 7:f04bde0ca846 135
backman 3:c5f2281b3ed2 136
backman 3:c5f2281b3ed2 137
backman 3:c5f2281b3ed2 138
backman 3:c5f2281b3ed2 139
backman 3:c5f2281b3ed2 140
backman 3:c5f2281b3ed2 141
backman 3:c5f2281b3ed2 142 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 143 //--------------------------------------------
backman 2:c51647d3c14d 144
backman 2:c51647d3c14d 145
backman 3:c5f2281b3ed2 146 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 147
backman 3:c5f2281b3ed2 148
backman 1:82bc25a7b68b 149
backman 3:c5f2281b3ed2 150
backman 1:82bc25a7b68b 151
backman 1:82bc25a7b68b 152
backman 3:c5f2281b3ed2 153 #endif
backman 1:82bc25a7b68b 154
backman 1:82bc25a7b68b 155
backman 1:82bc25a7b68b 156
backman 1:82bc25a7b68b 157
backman 1:82bc25a7b68b 158
backman 1:82bc25a7b68b 159
backman 1:82bc25a7b68b 160
backman 1:82bc25a7b68b 161
backman 1:82bc25a7b68b 162
backman 1:82bc25a7b68b 163
backman 1:82bc25a7b68b 164
backman 1:82bc25a7b68b 165
backman 1:82bc25a7b68b 166
backman 0:68c173249c01 167
backman 0:68c173249c01 168
backman 0:68c173249c01 169
backman 1:82bc25a7b68b 170 return 0;
backman 0:68c173249c01 171
backman 0:68c173249c01 172
backman 0:68c173249c01 173 }