Clark Lin / Mbed 2 deprecated 0709

QQQQQQQ

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

main.cpp@7:f04bde0ca846, 2014-07-07 (annotated)

Committer:
even
Date:
Mon Jul 07 15:23:50 2014 +0000
Revision:
7:f04bde0ca846
Parent:
6:b046d6ff3745
Child:
8:c2c3aee85c2d
half circle

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 7:f04bde0ca846 73
even 7:f04bde0ca846 74 /*if(turn == middle){
even 7:f04bde0ca846 75 if (last_turn < middle){
even 7:f04bde0ca846 76
even 7:f04bde0ca846 77 servo.set_angle(last_turn);
even 7:f04bde0ca846 78 wait_ms(500);
even 7:f04bde0ca846 79
even 7:f04bde0ca846 80 } else if (last_turn > middle){
even 7:f04bde0ca846 81
even 7:f04bde0ca846 82 servo.set_angle(last_turn);
even 7:f04bde0ca846 83 wait_ms(500);
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
even 7:f04bde0ca846 95 //output the psudo map
even 7:f04bde0ca846 96 for(int i = 127; i >= 0; i--)
even 7:f04bde0ca846 97 psudo_line[i] = '0';
even 7:f04bde0ca846 98
even 7:f04bde0ca846 99 for(int i = center-5; i < center+5; i++)
even 7:f04bde0ca846 100 psudo_line[i] = ' ';
even 7:f04bde0ca846 101
even 7:f04bde0ca846 102 for(int i = 117; i > 10; i--){
even 7:f04bde0ca846 103 pc.printf("%c", psudo_line[i]);
even 7:f04bde0ca846 104 }
even 7:f04bde0ca846 105 pc.printf("\r\n");
even 7:f04bde0ca846 106 pc.printf("black centerL: %d black_centerR: %d psudo_line: %d turn: %lf\r\n", black_centerL, black_centerR, center, turn);
even 7:f04bde0ca846 107
even 7:f04bde0ca846 108
even 7:f04bde0ca846 109 /*for(int i = 128; i > 64;i--){
backman 1:82bc25a7b68b 110 if(i==64)
backman 1:82bc25a7b68b 111 pc.printf("X");
backman 1:82bc25a7b68b 112 else
backman 2:c51647d3c14d 113 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 114 }
backman 2:c51647d3c14d 115 pc.printf(" || ");
backman 2:c51647d3c14d 116
even 7:f04bde0ca846 117 for(int i = 64; i > 0; i--){
backman 2:c51647d3c14d 118 if(i==64)
backman 2:c51647d3c14d 119 pc.printf("X");
backman 2:c51647d3c14d 120 else
backman 2:c51647d3c14d 121 pc.printf("%c", cam.sign_line_imageR[i]);
backman 1:82bc25a7b68b 122 }
even 7:f04bde0ca846 123 pc.printf("\r\n");
even 7:f04bde0ca846 124 pc.printf("black_centerL: %d black_centerR: %d\r\n", black_centerL, black_centerR);
even 7:f04bde0ca846 125 */
backman 3:c5f2281b3ed2 126 }
backman 3:c5f2281b3ed2 127
even 7:f04bde0ca846 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
backman 3:c5f2281b3ed2 135 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 136 //--------------------------------------------
backman 2:c51647d3c14d 137
backman 2:c51647d3c14d 138
backman 3:c5f2281b3ed2 139 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 140
backman 3:c5f2281b3ed2 141
backman 1:82bc25a7b68b 142
backman 3:c5f2281b3ed2 143
backman 1:82bc25a7b68b 144
backman 1:82bc25a7b68b 145
backman 3:c5f2281b3ed2 146 #endif
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 1:82bc25a7b68b 159
backman 0:68c173249c01 160
backman 0:68c173249c01 161
backman 0:68c173249c01 162
backman 1:82bc25a7b68b 163 return 0;
backman 0:68c173249c01 164
backman 0:68c173249c01 165
backman 0:68c173249c01 166 }