譯文 張 / Mbed 2 deprecated 7_7Boboobooo

123

Dependencies:   mbed

Fork of Boboobooo by Clark Lin

main.cpp@8:c2c3aee85c2d, 2014-07-08 (annotated)

Committer:
even
Date:
Tue Jul 08 04:36:09 2014 +0000
Revision:
8:c2c3aee85c2d
Parent:
7:f04bde0ca846
Child:
9:a891053657f3
7/7 end

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