譯文 張 / Mbed 2 deprecated 7_7Boboobooo

123

Dependencies:   mbed

Fork of Boboobooo by Clark Lin

main.cpp@10:e11f82b40d53, 2014-07-09 (annotated)

Committer:
even
Date:
Wed Jul 09 15:10:49 2014 +0000
Revision:
10:e11f82b40d53
Parent:
9:a891053657f3
Child:
11:9b4788de75fe
take a break (do not use)

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 double Kp = 0.000527, Ki = 0.025;
even 10:e11f82b40d53 24 int black_centerR, black_centerL, center, times = 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 7:f04bde0ca846 41 center = (black_centerL + black_centerR) / 2;
even 7:f04bde0ca846 42
even 7:f04bde0ca846 43 MotorA.rotate(0.2);
even 7:f04bde0ca846 44 MotorB.rotate(0.2);
even 7:f04bde0ca846 45
even 9:a891053657f3 46
even 10:e11f82b40d53 47 if(times % 10 == 0){
even 9:a891053657f3 48 avg = last_turn / 10;
even 10:e11f82b40d53 49 last_turn = 0;
even 10:e11f82b40d53 50
even 10:e11f82b40d53 51
even 10:e11f82b40d53 52
even 10:e11f82b40d53 53 if(times == 30){
even 10:e11f82b40d53 54 times = 0;
even 10:e11f82b40d53 55 if(avg < middle-0.003){
even 10:e11f82b40d53 56
even 10:e11f82b40d53 57 avg -= 0.002;
even 10:e11f82b40d53 58
even 10:e11f82b40d53 59 }
even 10:e11f82b40d53 60 else if(avg > middle + 0.003){
even 10:e11f82b40d53 61
even 10:e11f82b40d53 62 avg += 0.002;
even 10:e11f82b40d53 63 }
even 10:e11f82b40d53 64 }
even 9:a891053657f3 65 }
even 9:a891053657f3 66
even 7:f04bde0ca846 67 if(black_centerL == 128 && black_centerR == 0){//no line
even 7:f04bde0ca846 68
even 9:a891053657f3 69 turn = avg;
even 7:f04bde0ca846 70
even 7:f04bde0ca846 71 } else if (black_centerL == 128 && black_centerR != 0){//no left line
even 9:a891053657f3 72
even 9:a891053657f3 73
even 9:a891053657f3 74 /*error = 64 - black_centerR;
even 9:a891053657f3 75 turn = Kp * error + middle;*/
even 9:a891053657f3 76 turn = avg;
even 9:a891053657f3 77
even 7:f04bde0ca846 78
even 7:f04bde0ca846 79
even 7:f04bde0ca846 80 } else if (black_centerL != 128 && black_centerR == 0){//no right line
even 7:f04bde0ca846 81
even 9:a891053657f3 82
even 9:a891053657f3 83 /*error = 64 - black_centerL;
even 9:a891053657f3 84 turn = Kp * error + middle;*/
even 9:a891053657f3 85 turn = avg;
even 7:f04bde0ca846 86
even 7:f04bde0ca846 87 } else {
even 10:e11f82b40d53 88
even 7:f04bde0ca846 89 if(60 < center && center < 68){
even 7:f04bde0ca846 90
even 7:f04bde0ca846 91 turn = middle;
even 7:f04bde0ca846 92
even 7:f04bde0ca846 93 } else{
even 7:f04bde0ca846 94 error = 64 - center;
even 7:f04bde0ca846 95 turn = Kp * error + middle;
even 7:f04bde0ca846 96 }
even 7:f04bde0ca846 97 }
even 9:a891053657f3 98
even 9:a891053657f3 99 last_turn += turn;
even 9:a891053657f3 100 times++;
even 7:f04bde0ca846 101 servo.set_angle(turn);
even 7:f04bde0ca846 102
even 7:f04bde0ca846 103
even 7:f04bde0ca846 104 //output the psudo map
even 7:f04bde0ca846 105 for(int i = 127; i >= 0; i--)
even 7:f04bde0ca846 106 psudo_line[i] = '0';
even 7:f04bde0ca846 107
even 7:f04bde0ca846 108 for(int i = center-5; i < center+5; i++)
even 7:f04bde0ca846 109 psudo_line[i] = ' ';
even 7:f04bde0ca846 110
even 7:f04bde0ca846 111 for(int i = 117; i > 10; i--){
even 7:f04bde0ca846 112 pc.printf("%c", psudo_line[i]);
even 7:f04bde0ca846 113 }
even 7:f04bde0ca846 114 pc.printf("\r\n");
even 9:a891053657f3 115 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 116
even 7:f04bde0ca846 117
even 7:f04bde0ca846 118 /*for(int i = 128; i > 64;i--){
backman 1:82bc25a7b68b 119 if(i==64)
backman 1:82bc25a7b68b 120 pc.printf("X");
backman 1:82bc25a7b68b 121 else
backman 2:c51647d3c14d 122 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 123 }
backman 2:c51647d3c14d 124 pc.printf(" || ");
backman 2:c51647d3c14d 125
even 7:f04bde0ca846 126 for(int i = 64; i > 0; i--){
backman 2:c51647d3c14d 127 if(i==64)
backman 2:c51647d3c14d 128 pc.printf("X");
backman 2:c51647d3c14d 129 else
backman 2:c51647d3c14d 130 pc.printf("%c", cam.sign_line_imageR[i]);
backman 1:82bc25a7b68b 131 }
even 7:f04bde0ca846 132 pc.printf("\r\n");
even 7:f04bde0ca846 133 pc.printf("black_centerL: %d black_centerR: %d\r\n", black_centerL, black_centerR);
even 7:f04bde0ca846 134 */
backman 3:c5f2281b3ed2 135 }
backman 3:c5f2281b3ed2 136
even 7:f04bde0ca846 137
backman 3:c5f2281b3ed2 138
backman 3:c5f2281b3ed2 139
backman 3:c5f2281b3ed2 140
backman 3:c5f2281b3ed2 141
backman 3:c5f2281b3ed2 142
backman 3:c5f2281b3ed2 143
backman 3:c5f2281b3ed2 144 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 145 //--------------------------------------------
backman 2:c51647d3c14d 146
backman 2:c51647d3c14d 147
backman 3:c5f2281b3ed2 148 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 149
backman 3:c5f2281b3ed2 150
backman 1:82bc25a7b68b 151
backman 3:c5f2281b3ed2 152
backman 1:82bc25a7b68b 153
backman 1:82bc25a7b68b 154
backman 3:c5f2281b3ed2 155 #endif
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 1:82bc25a7b68b 167
backman 1:82bc25a7b68b 168
backman 0:68c173249c01 169
backman 0:68c173249c01 170
backman 0:68c173249c01 171
backman 1:82bc25a7b68b 172 return 0;
backman 0:68c173249c01 173
backman 0:68c173249c01 174
backman 0:68c173249c01 175 }