Yi Lin Chen / Mbed 2 deprecated Boboobooo

shared

Dependencies:   mbed-rtos mbed

Fork of Boboobooo by kao yi

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

Committer:
Kruskal
Date:
Mon Jul 07 06:52:03 2014 +0000
Revision:
7:fe8665daf3e7
Parent:
6:132facf9b42d
Child:
8:089b778962c4
bobobo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
backman 1:82bc25a7b68b 2 #include "servo_api.h"
backman 1:82bc25a7b68b 3 #include "camera_api.h"
Kruskal 7:fe8665daf3e7 4 #include "motor_api.h"
backman 3:c5f2281b3ed2 5
backman 3:c5f2281b3ed2 6 #define Debug_cam_uart
Kruskal 7:fe8665daf3e7 7 #define buffer_size 3
backman 1:82bc25a7b68b 8 Serial pc(USBTX, USBRX);
backman 1:82bc25a7b68b 9 BX_servo servo;
Kruskal 7:fe8665daf3e7 10 BX_camera cam;
Kruskal 7:fe8665daf3e7 11 BX_motor motorA('A');
Kruskal 7:fe8665daf3e7 12 BX_motor motorB('B');
backman 3:c5f2281b3ed2 13
backman 3:c5f2281b3ed2 14
backman 0:68c173249c01 15 int main() {
backman 0:68c173249c01 16
backman 1:82bc25a7b68b 17 /*
backman 1:82bc25a7b68b 18 int black_va;
backman 1:82bc25a7b68b 19 int white_va;
backman 1:82bc25a7b68b 20 */
Kruskal 7:fe8665daf3e7 21 int left_point_buffer[buffer_size] = {0,0,0};
Kruskal 7:fe8665daf3e7 22 int right_point_buffer[buffer_size] = {0,0,0};
Kruskal 7:fe8665daf3e7 23 int left_length_buffer[buffer_size] = {0,0,0};
Kruskal 7:fe8665daf3e7 24 int right_length_buffer[buffer_size] = {0,0,0};
Kruskal 7:fe8665daf3e7 25 int error_buffer[buffer_size]= {0,0,0};
Kruskal 7:fe8665daf3e7 26 bool isLeftOut = false;
Kruskal 7:fe8665daf3e7 27 bool isRightOut = false;
Kruskal 7:fe8665daf3e7 28 servo.set_angle(0);
Kruskal 7:fe8665daf3e7 29
backman 3:c5f2281b3ed2 30 #ifdef Debug_cam_uart
backman 1:82bc25a7b68b 31 pc.baud(115200);
backman 1:82bc25a7b68b 32
Kruskal 7:fe8665daf3e7 33 motorA.rotate(0.5);
Kruskal 7:fe8665daf3e7 34 motorB.rotate(0.5);
backman 1:82bc25a7b68b 35
backman 3:c5f2281b3ed2 36 while(1){
backman 1:82bc25a7b68b 37
Kruskal 7:fe8665daf3e7 38 cam.read();
Kruskal 7:fe8665daf3e7 39
Kruskal 7:fe8665daf3e7 40 //====== find left line's right edging ======
Kruskal 7:fe8665daf3e7 41 bool left_isFistPoint = true;
Kruskal 7:fe8665daf3e7 42 int left_temp_length = 0;
Kruskal 7:fe8665daf3e7 43 int left_max_length = 0;
Kruskal 7:fe8665daf3e7 44 int left_temp_point = 0;
Kruskal 7:fe8665daf3e7 45 int left_target_point = 0;
Kruskal 7:fe8665daf3e7 46 int left_white_count = 0;
Kruskal 7:fe8665daf3e7 47 int left_white_threshold = 2;
Kruskal 7:fe8665daf3e7 48 // scan camera data from right to left
Kruskal 7:fe8665daf3e7 49 for(int i=117;i>=10;i--){
Kruskal 7:fe8665daf3e7 50 // when scan result is black and it is a start point
Kruskal 7:fe8665daf3e7 51 if(cam.sign_line_imageL[i] == ' ' && left_isFistPoint){
Kruskal 7:fe8665daf3e7 52 left_temp_point = i;
Kruskal 7:fe8665daf3e7 53 left_isFistPoint = false;
Kruskal 7:fe8665daf3e7 54 if(i == 10 && (left_max_length < left_temp_length)){
Kruskal 7:fe8665daf3e7 55 left_max_length = left_temp_length;
Kruskal 7:fe8665daf3e7 56 left_target_point = left_temp_point;
Kruskal 7:fe8665daf3e7 57 }
Kruskal 7:fe8665daf3e7 58 }
Kruskal 7:fe8665daf3e7 59 // when scan result is black and it is not a start point
Kruskal 7:fe8665daf3e7 60 else if(cam.sign_line_imageL[i] == ' ' && !left_isFistPoint){
Kruskal 7:fe8665daf3e7 61 left_temp_length++;
Kruskal 7:fe8665daf3e7 62 if(i == 10 && (left_max_length < left_temp_length)){
Kruskal 7:fe8665daf3e7 63 left_max_length = left_temp_length;
Kruskal 7:fe8665daf3e7 64 left_target_point = left_temp_point;
Kruskal 7:fe8665daf3e7 65 }
Kruskal 7:fe8665daf3e7 66 }
Kruskal 7:fe8665daf3e7 67 // when scan result is white and there is no black behind it
Kruskal 7:fe8665daf3e7 68 else if(cam.sign_line_imageL[i] != ' ' && left_isFistPoint){
Kruskal 7:fe8665daf3e7 69 //do nothng
backman 3:c5f2281b3ed2 70 }
Kruskal 7:fe8665daf3e7 71 //when scan result is white and there are some black behind it
Kruskal 7:fe8665daf3e7 72 else if(cam.sign_line_imageL[i] != ' ' && !left_isFistPoint){
Kruskal 7:fe8665daf3e7 73 left_white_count++;
Kruskal 7:fe8665daf3e7 74 //this white is still in the threshold
Kruskal 7:fe8665daf3e7 75 if(left_white_count <= left_white_threshold){
Kruskal 7:fe8665daf3e7 76 left_temp_length++;
Kruskal 7:fe8665daf3e7 77 if(i == 10 && (left_max_length < left_temp_length)){
Kruskal 7:fe8665daf3e7 78 left_max_length = left_temp_length;
Kruskal 7:fe8665daf3e7 79 left_target_point = left_temp_point;
Kruskal 7:fe8665daf3e7 80 }
Kruskal 7:fe8665daf3e7 81 }
Kruskal 7:fe8665daf3e7 82 //this white is out of threshold
Kruskal 7:fe8665daf3e7 83 else{
Kruskal 7:fe8665daf3e7 84 left_isFistPoint = true;
Kruskal 7:fe8665daf3e7 85 if(left_max_length < left_temp_length){
Kruskal 7:fe8665daf3e7 86 left_max_length = left_temp_length;
Kruskal 7:fe8665daf3e7 87 left_target_point = left_temp_point;
Kruskal 7:fe8665daf3e7 88 }
Kruskal 7:fe8665daf3e7 89 left_temp_length = 0;
Kruskal 7:fe8665daf3e7 90 left_white_count = 0;
Kruskal 7:fe8665daf3e7 91 }
Kruskal 7:fe8665daf3e7 92 }
Kruskal 7:fe8665daf3e7 93 }
Kruskal 7:fe8665daf3e7 94
Kruskal 7:fe8665daf3e7 95 //average data
Kruskal 7:fe8665daf3e7 96 int left_average_point = 0;
Kruskal 7:fe8665daf3e7 97 int left_average_length = 0;
Kruskal 7:fe8665daf3e7 98
Kruskal 7:fe8665daf3e7 99 //check line
Kruskal 7:fe8665daf3e7 100 bool isLeftAllWhite = true;
Kruskal 7:fe8665daf3e7 101 int left_black_count = 0;
Kruskal 7:fe8665daf3e7 102 for(int i=10;i<=117;i++){
Kruskal 7:fe8665daf3e7 103 if(cam.sign_line_imageL[i] == ' ')
Kruskal 7:fe8665daf3e7 104 {
Kruskal 7:fe8665daf3e7 105 left_black_count++;
Kruskal 7:fe8665daf3e7 106 if( left_black_count >= 3 )
Kruskal 7:fe8665daf3e7 107 {
Kruskal 7:fe8665daf3e7 108 isLeftAllWhite = false;
Kruskal 7:fe8665daf3e7 109 }
Kruskal 7:fe8665daf3e7 110 }
Kruskal 7:fe8665daf3e7 111 }
Kruskal 7:fe8665daf3e7 112 if(isLeftAllWhite){
Kruskal 7:fe8665daf3e7 113 //do not update the data
Kruskal 7:fe8665daf3e7 114 //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2])/buffer_size;
Kruskal 7:fe8665daf3e7 115 //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2])/buffer_size;
Kruskal 7:fe8665daf3e7 116 left_average_point = left_point_buffer[buffer_size-1];
Kruskal 7:fe8665daf3e7 117 left_average_length = left_length_buffer[buffer_size-1];
Kruskal 7:fe8665daf3e7 118
Kruskal 7:fe8665daf3e7 119 }
Kruskal 7:fe8665daf3e7 120 else{
Kruskal 7:fe8665daf3e7 121 //average the history data
Kruskal 7:fe8665daf3e7 122 //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_target_point)/(buffer_size+1);
Kruskal 7:fe8665daf3e7 123 //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2] + left_max_length)/(buffer_size+1);
Kruskal 7:fe8665daf3e7 124
Kruskal 7:fe8665daf3e7 125 left_average_point = left_target_point;
Kruskal 7:fe8665daf3e7 126 left_average_length = left_max_length;
backman 3:c5f2281b3ed2 127
Kruskal 7:fe8665daf3e7 128 //update the buffer
Kruskal 7:fe8665daf3e7 129 for(int i=0;i<buffer_size - 1;i++){
Kruskal 7:fe8665daf3e7 130 left_point_buffer[i] = left_point_buffer[i+1];
Kruskal 7:fe8665daf3e7 131 left_length_buffer[i] = left_length_buffer[i+1];
Kruskal 7:fe8665daf3e7 132 }
Kruskal 7:fe8665daf3e7 133 left_point_buffer[buffer_size-1] = left_average_point;
Kruskal 7:fe8665daf3e7 134 left_length_buffer[buffer_size-1] = left_average_length;
Kruskal 7:fe8665daf3e7 135 }
backman 1:82bc25a7b68b 136
backman 0:68c173249c01 137
Kruskal 7:fe8665daf3e7 138 //====== find right line's left edging ======
Kruskal 7:fe8665daf3e7 139 bool right_isFistPoint = true;
Kruskal 7:fe8665daf3e7 140 int right_temp_length = 0;
Kruskal 7:fe8665daf3e7 141 int right_max_length = 0;
Kruskal 7:fe8665daf3e7 142 int right_temp_point = 0;
Kruskal 7:fe8665daf3e7 143 int right_target_point = 0;
Kruskal 7:fe8665daf3e7 144 int right_white_count = 0;
Kruskal 7:fe8665daf3e7 145 int right_white_threshold = 2;
Kruskal 7:fe8665daf3e7 146 //scan camera data from right to left
Kruskal 7:fe8665daf3e7 147 for(int i=10;i<=117;i++){
Kruskal 7:fe8665daf3e7 148 //when scan result is black and it is a start point
Kruskal 7:fe8665daf3e7 149 if(cam.sign_line_imageR[i] == ' ' && right_isFistPoint){
Kruskal 7:fe8665daf3e7 150 right_temp_point = i;
Kruskal 7:fe8665daf3e7 151 right_isFistPoint = false;
Kruskal 7:fe8665daf3e7 152 if( i == 117 && (right_max_length < right_temp_length)){
Kruskal 7:fe8665daf3e7 153 right_max_length = right_temp_length;
Kruskal 7:fe8665daf3e7 154 right_target_point = right_temp_point;
Kruskal 7:fe8665daf3e7 155 }
Kruskal 7:fe8665daf3e7 156 }
Kruskal 7:fe8665daf3e7 157 //when scan result is black and it is not a start point
Kruskal 7:fe8665daf3e7 158 else if(cam.sign_line_imageR[i] == ' ' && !right_isFistPoint){
Kruskal 7:fe8665daf3e7 159 right_temp_length++;
Kruskal 7:fe8665daf3e7 160 if( i == 117 && (right_max_length < right_temp_length)){
Kruskal 7:fe8665daf3e7 161 right_max_length = right_temp_length;
Kruskal 7:fe8665daf3e7 162 right_target_point = right_temp_point;
Kruskal 7:fe8665daf3e7 163 }
Kruskal 7:fe8665daf3e7 164 }
Kruskal 7:fe8665daf3e7 165 //when scan result is white and there is no black behind it
Kruskal 7:fe8665daf3e7 166 else if(cam.sign_line_imageR[i] != ' ' && right_isFistPoint){
Kruskal 7:fe8665daf3e7 167 //do nothng
Kruskal 7:fe8665daf3e7 168 }
Kruskal 7:fe8665daf3e7 169 //when scan result is white and there are some black behind it
Kruskal 7:fe8665daf3e7 170 else if(cam.sign_line_imageR[i] != ' ' && !right_isFistPoint){
Kruskal 7:fe8665daf3e7 171 right_white_count++;
Kruskal 7:fe8665daf3e7 172 //this white is still in the threshold
Kruskal 7:fe8665daf3e7 173 if(right_white_count <= right_white_threshold){
Kruskal 7:fe8665daf3e7 174 right_temp_length++;
Kruskal 7:fe8665daf3e7 175 if( i == 117 && (right_max_length < right_temp_length)){
Kruskal 7:fe8665daf3e7 176 right_max_length = right_temp_length;
Kruskal 7:fe8665daf3e7 177 right_target_point = right_temp_point;
Kruskal 7:fe8665daf3e7 178 }
Kruskal 7:fe8665daf3e7 179 }
Kruskal 7:fe8665daf3e7 180 //this white is out of threshold
Kruskal 7:fe8665daf3e7 181 else{
Kruskal 7:fe8665daf3e7 182 right_isFistPoint = true;
Kruskal 7:fe8665daf3e7 183 if(right_max_length < right_temp_length){
Kruskal 7:fe8665daf3e7 184 right_max_length = right_temp_length;
Kruskal 7:fe8665daf3e7 185 right_target_point = right_temp_point;
Kruskal 7:fe8665daf3e7 186 }
Kruskal 7:fe8665daf3e7 187 right_temp_length = 0;
Kruskal 7:fe8665daf3e7 188 right_white_count = 0;
Kruskal 7:fe8665daf3e7 189 }
Kruskal 7:fe8665daf3e7 190 }
Kruskal 7:fe8665daf3e7 191 }
Kruskal 7:fe8665daf3e7 192
Kruskal 7:fe8665daf3e7 193 //average data
Kruskal 7:fe8665daf3e7 194 int right_average_point = 0;
Kruskal 7:fe8665daf3e7 195 int right_average_length = 0;
Kruskal 7:fe8665daf3e7 196 //check line
Kruskal 7:fe8665daf3e7 197 bool isRightAllWhite = true;
Kruskal 7:fe8665daf3e7 198 int right_black_count = 0;
Kruskal 7:fe8665daf3e7 199 for(int i=10;i<=117;i++){
Kruskal 7:fe8665daf3e7 200 if(cam.sign_line_imageR[i] == ' ')
Kruskal 7:fe8665daf3e7 201 {
Kruskal 7:fe8665daf3e7 202 right_black_count++;
Kruskal 7:fe8665daf3e7 203 if( right_black_count >= 3 )
Kruskal 7:fe8665daf3e7 204 {
Kruskal 7:fe8665daf3e7 205 isRightAllWhite = false;
Kruskal 7:fe8665daf3e7 206 }
Kruskal 7:fe8665daf3e7 207 }
Kruskal 7:fe8665daf3e7 208 }
Kruskal 7:fe8665daf3e7 209
Kruskal 7:fe8665daf3e7 210 if(isRightAllWhite){
Kruskal 7:fe8665daf3e7 211 //do not update the buffer
Kruskal 7:fe8665daf3e7 212 //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2])/buffer_size;
Kruskal 7:fe8665daf3e7 213 //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2])/buffer_size;
Kruskal 7:fe8665daf3e7 214 right_average_point = right_point_buffer[buffer_size-1];
Kruskal 7:fe8665daf3e7 215 right_average_length = right_length_buffer[buffer_size-1];
Kruskal 7:fe8665daf3e7 216 }
Kruskal 7:fe8665daf3e7 217 else{
Kruskal 7:fe8665daf3e7 218 //average the history data
Kruskal 7:fe8665daf3e7 219 //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_target_point)/(buffer_size+1);
Kruskal 7:fe8665daf3e7 220 //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2] + right_max_length)/(buffer_size+1);
Kruskal 7:fe8665daf3e7 221 right_average_point = right_target_point;
Kruskal 7:fe8665daf3e7 222 right_average_length = right_max_length;
Kruskal 7:fe8665daf3e7 223
Kruskal 7:fe8665daf3e7 224 //update the buffer
Kruskal 7:fe8665daf3e7 225 for(int i=0;i<buffer_size-1;i++){
Kruskal 7:fe8665daf3e7 226 right_point_buffer[i] = right_point_buffer[i+1];
Kruskal 7:fe8665daf3e7 227 right_length_buffer[i] = right_length_buffer[i+1];
Kruskal 7:fe8665daf3e7 228 }
Kruskal 7:fe8665daf3e7 229 right_point_buffer[buffer_size-1] = right_average_point;
Kruskal 7:fe8665daf3e7 230 right_length_buffer[buffer_size-1] = right_average_length;
Kruskal 7:fe8665daf3e7 231 }
Kruskal 7:fe8665daf3e7 232
Kruskal 7:fe8665daf3e7 233 //print left camera result
Kruskal 7:fe8665daf3e7 234
Kruskal 7:fe8665daf3e7 235 for(int i=0;i<=127;i++){
Kruskal 7:fe8665daf3e7 236 if( i >= left_average_point-left_average_length && i<=left_average_point)
Kruskal 7:fe8665daf3e7 237 pc.printf(" ");
Kruskal 7:fe8665daf3e7 238 else
Kruskal 7:fe8665daf3e7 239 pc.printf("x");
Kruskal 7:fe8665daf3e7 240 }
Kruskal 7:fe8665daf3e7 241
Kruskal 7:fe8665daf3e7 242 //print right camera result
Kruskal 7:fe8665daf3e7 243
Kruskal 7:fe8665daf3e7 244 for(int i=0;i<=127;i++){
Kruskal 7:fe8665daf3e7 245 if( i <= right_average_point+right_average_length && i>=right_average_point)
Kruskal 7:fe8665daf3e7 246 pc.printf(" ");
Kruskal 7:fe8665daf3e7 247 else
Kruskal 7:fe8665daf3e7 248 pc.printf("x");
Kruskal 7:fe8665daf3e7 249 }
backman 0:68c173249c01 250
backman 0:68c173249c01 251
Kruskal 7:fe8665daf3e7 252 pc.printf("\n L_center : %d , R_center : %d \r\n", left_average_point,right_average_point);
Kruskal 7:fe8665daf3e7 253 pc.printf("\r\n");
backman 0:68c173249c01 254
Kruskal 7:fe8665daf3e7 255 //PID -- P
Kruskal 7:fe8665daf3e7 256 int error = 0;
Kruskal 7:fe8665daf3e7 257 int turn = 0;
Kruskal 7:fe8665daf3e7 258 int center = 55;
Kruskal 7:fe8665daf3e7 259 float Kp = 0;
Kruskal 7:fe8665daf3e7 260 float turn_angle = 0;
Kruskal 7:fe8665daf3e7 261 int reference_point= 0;
Kruskal 7:fe8665daf3e7 262
Kruskal 7:fe8665daf3e7 263 if(isLeftAllWhite && !isRightAllWhite){
Kruskal 7:fe8665daf3e7 264 error = 40;
Kruskal 7:fe8665daf3e7 265 turn = 90;
Kruskal 7:fe8665daf3e7 266 reference_point = right_average_point;
Kruskal 7:fe8665daf3e7 267 }
Kruskal 7:fe8665daf3e7 268 else if(!isLeftAllWhite && isRightAllWhite){
Kruskal 7:fe8665daf3e7 269 error = 40;
Kruskal 7:fe8665daf3e7 270 turn = 90;
Kruskal 7:fe8665daf3e7 271 reference_point = left_average_point;
Kruskal 7:fe8665daf3e7 272 }
Kruskal 7:fe8665daf3e7 273 else if(isLeftAllWhite && isRightAllWhite){
Kruskal 7:fe8665daf3e7 274 error = 30;
Kruskal 7:fe8665daf3e7 275 turn = 90;
Kruskal 7:fe8665daf3e7 276 if(left_average_point < 40 && right_average_point < 40)
Kruskal 7:fe8665daf3e7 277 reference_point = 0;
Kruskal 7:fe8665daf3e7 278 else if(left_average_point > 70 && right_average_point > 70)
Kruskal 7:fe8665daf3e7 279 reference_point = 90;
Kruskal 7:fe8665daf3e7 280
Kruskal 7:fe8665daf3e7 281 //pc.printf("\n allllllllllllllllllllll whiteeeeeeeeeeeeeee \n");
Kruskal 7:fe8665daf3e7 282
Kruskal 7:fe8665daf3e7 283 }
Kruskal 7:fe8665daf3e7 284 else if(!isLeftAllWhite && !isRightAllWhite){
Kruskal 7:fe8665daf3e7 285 error = 50;
Kruskal 7:fe8665daf3e7 286 turn = 90;
Kruskal 7:fe8665daf3e7 287 center = 108;
Kruskal 7:fe8665daf3e7 288 reference_point = (right_average_point + 108 + left_average_point)/2;
Kruskal 7:fe8665daf3e7 289 }
backman 0:68c173249c01 290
Kruskal 7:fe8665daf3e7 291 Kp = turn/error;
Kruskal 7:fe8665daf3e7 292
Kruskal 7:fe8665daf3e7 293 //PID -- I
Kruskal 7:fe8665daf3e7 294 /*for(int i=0;i<4;i++){
Kruskal 7:fe8665daf3e7 295 error_buffer[i] = error_buffer[i+1];
Kruskal 7:fe8665daf3e7 296 }
Kruskal 7:fe8665daf3e7 297 error_buffer[4] = reference_point - center;
Kruskal 7:fe8665daf3e7 298 float integral = 0;
Kruskal 7:fe8665daf3e7 299 float Ki = 1.1f;
Kruskal 7:fe8665daf3e7 300 for(int i=0;i<5;i++){
Kruskal 7:fe8665daf3e7 301 integral = (2/3)*integral + error_buffer[i];
Kruskal 7:fe8665daf3e7 302 }
Kruskal 7:fe8665daf3e7 303 turn_angle = (int) (Kp*(reference_point - center) + Ki*integral);*/
Kruskal 7:fe8665daf3e7 304
Kruskal 7:fe8665daf3e7 305 turn_angle = (int) (Kp*(reference_point - center));
Kruskal 7:fe8665daf3e7 306 if(turn_angle >= 50)
Kruskal 7:fe8665daf3e7 307 turn_angle = 50;
Kruskal 7:fe8665daf3e7 308 if(turn_angle <= -50)
Kruskal 7:fe8665daf3e7 309 turn_angle = -50;
Kruskal 7:fe8665daf3e7 310
Kruskal 7:fe8665daf3e7 311 servo.set_angle(turn_angle);
Kruskal 7:fe8665daf3e7 312 }
Kruskal 7:fe8665daf3e7 313
Kruskal 7:fe8665daf3e7 314
Kruskal 7:fe8665daf3e7 315
Kruskal 7:fe8665daf3e7 316 #endif
Kruskal 7:fe8665daf3e7 317
Kruskal 7:fe8665daf3e7 318 return 0;
Kruskal 7:fe8665daf3e7 319
Kruskal 7:fe8665daf3e7 320 }