Clark Lin / Mbed 2 deprecated 0709

QQQQQQQ

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

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

Committer:
physicsgood
Date:
Wed Jul 09 07:34:23 2014 +0000
Revision:
9:c318ee86d6e9
Parent:
8:c2c3aee85c2d 
0709

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
physicsgood 9:c318ee86d6e9 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() {
backman 1:82bc25a7b68b 20 /*
backman 1:82bc25a7b68b 21 int black_va;
backman 1:82bc25a7b68b 22 int white_va;
backman 1:82bc25a7b68b 23 */
physicsgood 9:c318ee86d6e9 24 double left = 0.025, middle = 0.041, right = 0.057;
physicsgood 9:c318ee86d6e9 25 //double left = 0.04, middle = 0.058, right = 0.076;
physicsgood 9:c318ee86d6e9 26 double error, turn, last_turn =0.0;
physicsgood 9:c318ee86d6e9 27 double Kp = 0.00025;
physicsgood 9:c318ee86d6e9 28 double v = 0.3;
even 7:f04bde0ca846 29 int black_centerR, black_centerL, center;
even 7:f04bde0ca846 30
even 7:f04bde0ca846 31 char psudo_line[128];
physicsgood 9:c318ee86d6e9 32 int times = 0;
physicsgood 9:c318ee86d6e9 33 double avg = middle;
backman 1:82bc25a7b68b 34
backman 3:c5f2281b3ed2 35 #ifdef Debug_cam_uart
backman 1:82bc25a7b68b 36 pc.baud(115200);
backman 1:82bc25a7b68b 37
backman 1:82bc25a7b68b 38
backman 3:c5f2281b3ed2 39 while(1){
backman 1:82bc25a7b68b 40
physicsgood 6:b046d6ff3745 41 cam.read();
physicsgood 9:c318ee86d6e9 42
even 7:f04bde0ca846 43
physicsgood 9:c318ee86d6e9 44 //black_centerL = cam.black_centerL();
physicsgood 9:c318ee86d6e9 45 center = cam.black_centerL();
even 7:f04bde0ca846 46 black_centerR = cam.black_centerR();
physicsgood 9:c318ee86d6e9 47 /*int R = 64 - black_centerR;
physicsgood 9:c318ee86d6e9 48 int L = 64 - black_centerL;
physicsgood 9:c318ee86d6e9 49 if( L > 0 && R > 0){
physicsgood 9:c318ee86d6e9 50 center = (L >= R )? black_centerL : black_centerR;
physicsgood 9:c318ee86d6e9 51 }
physicsgood 9:c318ee86d6e9 52 else if(black_centerL < 0 && black_centerR > 0){
physicsgood 9:c318ee86d6e9 53 center = ((-1)*L >= R )? black_centerL : black_centerR;
physicsgood 9:c318ee86d6e9 54 }
physicsgood 9:c318ee86d6e9 55 else if(black_centerL > 0 && black_centerR < 0){
physicsgood 9:c318ee86d6e9 56 center = (L >= (-1)*R )? black_centerL : black_centerR;
physicsgood 9:c318ee86d6e9 57 }
physicsgood 9:c318ee86d6e9 58 else{
physicsgood 9:c318ee86d6e9 59 center = ((-1)*L >= (-1)*R )? black_centerL : black_centerR;
physicsgood 9:c318ee86d6e9 60 }*/
even 7:f04bde0ca846 61
physicsgood 9:c318ee86d6e9 62 //center = (black_centerL + black_centerR) / 2;
physicsgood 9:c318ee86d6e9 63 /*if(black_centerL == 128 && black_centerR == 0){//no line
even 7:f04bde0ca846 64
even 7:f04bde0ca846 65 turn = middle;
even 7:f04bde0ca846 66
even 7:f04bde0ca846 67 } else if (black_centerL == 128 && black_centerR != 0){//no left line
even 7:f04bde0ca846 68
even 7:f04bde0ca846 69 turn = (middle + left) / 2;
even 7:f04bde0ca846 70
even 7:f04bde0ca846 71
even 7:f04bde0ca846 72 } else if (black_centerL != 128 && black_centerR == 0){//no right line
even 7:f04bde0ca846 73
even 7:f04bde0ca846 74 turn = (middle + right) / 2;
even 7:f04bde0ca846 75
even 7:f04bde0ca846 76 } else {
even 7:f04bde0ca846 77 if(60 < center && center < 68){
even 7:f04bde0ca846 78
even 7:f04bde0ca846 79 turn = middle;
even 7:f04bde0ca846 80
even 7:f04bde0ca846 81 } else{
even 7:f04bde0ca846 82 error = 64 - center;
even 7:f04bde0ca846 83 turn = Kp * error + middle;
even 7:f04bde0ca846 84 }
physicsgood 9:c318ee86d6e9 85 }*/
physicsgood 9:c318ee86d6e9 86 if(center<68 &&center>60)
physicsgood 9:c318ee86d6e9 87 turn = middle;
physicsgood 9:c318ee86d6e9 88 else
physicsgood 9:c318ee86d6e9 89 turn = middle + Kp *(64-center);
physicsgood 9:c318ee86d6e9 90
even 7:f04bde0ca846 91
even 7:f04bde0ca846 92 servo.set_angle(turn);
physicsgood 9:c318ee86d6e9 93 //MotorA.rotate(0.3);
physicsgood 9:c318ee86d6e9 94 //MotorB.rotate(0.3);
physicsgood 9:c318ee86d6e9 95 //last_turn += turn;
physicsgood 9:c318ee86d6e9 96 //times++;
even 7:f04bde0ca846 97 //output the psudo map
physicsgood 9:c318ee86d6e9 98 /*for(int i = 127; i >= 0; i--)
even 7:f04bde0ca846 99 psudo_line[i] = '0';
even 7:f04bde0ca846 100
even 7:f04bde0ca846 101 for(int i = center-5; i < center+5; i++)
even 7:f04bde0ca846 102 psudo_line[i] = ' ';
even 7:f04bde0ca846 103
even 7:f04bde0ca846 104 for(int i = 117; i > 10; i--){
even 7:f04bde0ca846 105 pc.printf("%c", psudo_line[i]);
even 7:f04bde0ca846 106 }
even 7:f04bde0ca846 107 pc.printf("\r\n");
even 7:f04bde0ca846 108 pc.printf("black centerL: %d black_centerR: %d psudo_line: %d turn: %lf\r\n", black_centerL, black_centerR, center, turn);
physicsgood 9:c318ee86d6e9 109 */
even 7:f04bde0ca846 110
physicsgood 9:c318ee86d6e9 111 for(int i = 128; i > 64;i--){
backman 1:82bc25a7b68b 112 if(i==64)
backman 1:82bc25a7b68b 113 pc.printf("X");
backman 1:82bc25a7b68b 114 else
backman 2:c51647d3c14d 115 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 116 }
backman 2:c51647d3c14d 117 pc.printf(" || ");
backman 2:c51647d3c14d 118
even 7:f04bde0ca846 119 for(int i = 64; i > 0; i--){
backman 2:c51647d3c14d 120 if(i==64)
backman 2:c51647d3c14d 121 pc.printf("X");
backman 2:c51647d3c14d 122 else
backman 2:c51647d3c14d 123 pc.printf("%c", cam.sign_line_imageR[i]);
backman 1:82bc25a7b68b 124 }
even 7:f04bde0ca846 125 pc.printf("\r\n");
physicsgood 9:c318ee86d6e9 126 pc.printf("black_centerL: %d black_centerR: %d center: %d turn: %lf\r\n", black_centerL, black_centerR,center,turn);
physicsgood 9:c318ee86d6e9 127
backman 3:c5f2281b3ed2 128 }
physicsgood 9:c318ee86d6e9 129
backman 3:c5f2281b3ed2 130
even 7:f04bde0ca846 131
backman 3:c5f2281b3ed2 132
backman 3:c5f2281b3ed2 133
backman 3:c5f2281b3ed2 134
backman 3:c5f2281b3ed2 135
backman 3:c5f2281b3ed2 136
backman 3:c5f2281b3ed2 137
backman 3:c5f2281b3ed2 138 // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
backman 1:82bc25a7b68b 139 //--------------------------------------------
backman 2:c51647d3c14d 140
backman 2:c51647d3c14d 141
backman 3:c5f2281b3ed2 142 // servo.set_angle(( (64.0-center) /64.0 )*90 );
backman 2:c51647d3c14d 143
backman 3:c5f2281b3ed2 144
backman 1:82bc25a7b68b 145
backman 3:c5f2281b3ed2 146
backman 1:82bc25a7b68b 147
backman 1:82bc25a7b68b 148
backman 3:c5f2281b3ed2 149 #endif
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 1:82bc25a7b68b 160
backman 1:82bc25a7b68b 161
backman 1:82bc25a7b68b 162
backman 0:68c173249c01 163
backman 0:68c173249c01 164
backman 0:68c173249c01 165
backman 1:82bc25a7b68b 166 return 0;
backman 0:68c173249c01 167
backman 0:68c173249c01 168
backman 0:68c173249c01 169 }