Yi Lin Chen / Mbed 2 deprecated Boboobooo

shared

Dependencies:   mbed-rtos mbed

Fork of Boboobooo by kao yi

main.cpp@8:089b778962c4, 2014-10-31 (annotated)

Committer:
Kruskal
Date:
Fri Oct 31 10:54:51 2014 +0000
Revision:
8:089b778962c4
Parent:
7:fe8665daf3e7 
:D

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
Kruskal 8:089b778962c4 2 #include "rtos.h"
backman 1:82bc25a7b68b 3 #include "servo_api.h"
backman 1:82bc25a7b68b 4 #include "camera_api.h"
Kruskal 7:fe8665daf3e7 5 #include "motor_api.h"
Kruskal 8:089b778962c4 6 #include "TSISensor.h"
Kruskal 8:089b778962c4 7 #include "pot.h"
backman 3:c5f2281b3ed2 8 #define Debug_cam_uart
Kruskal 8:089b778962c4 9 #define buffer_size 6
Kruskal 8:089b778962c4 10 #define e_buffer_size 50
Kruskal 8:089b778962c4 11 //os
Kruskal 8:089b778962c4 12 Mutex stdio_mutex;
Kruskal 8:089b778962c4 13 BX_pot pot1('1');
Kruskal 8:089b778962c4 14 TSISensor tsiSensor;
Kruskal 8:089b778962c4 15
backman 1:82bc25a7b68b 16 Serial pc(USBTX, USBRX);
backman 1:82bc25a7b68b 17 BX_servo servo;
Kruskal 7:fe8665daf3e7 18 BX_camera cam;
Kruskal 7:fe8665daf3e7 19 BX_motor motorA('A');
Kruskal 7:fe8665daf3e7 20 BX_motor motorB('B');
backman 3:c5f2281b3ed2 21
Kruskal 8:089b778962c4 22 int left_point_buffer[buffer_size] = {0,0,0,0,0,0};
Kruskal 8:089b778962c4 23 int right_point_buffer[buffer_size] = {0,0,0,0,0,0};
Kruskal 8:089b778962c4 24 int error_buffer[e_buffer_size] ={0};
Kruskal 8:089b778962c4 25 float angle_buffer[50]={0};
Kruskal 8:089b778962c4 26 int state = 0; //0: no line, 1:right line, 2:left line
Kruskal 8:089b778962c4 27 int last_state = 0;
Kruskal 8:089b778962c4 28 float turnAngle = 0.073;
Kruskal 8:089b778962c4 29 float Kp = 0.0;
backman 3:c5f2281b3ed2 30
Kruskal 7:fe8665daf3e7 31
Kruskal 8:089b778962c4 32 void servo_thread(void const *args){
Kruskal 8:089b778962c4 33 while(1){
Kruskal 8:089b778962c4 34 for(int i=0;i<49;i++){
Kruskal 8:089b778962c4 35 angle_buffer[i] = angle_buffer[i+1];
Kruskal 8:089b778962c4 36 }
Kruskal 8:089b778962c4 37 angle_buffer[49] = turnAngle;
Kruskal 8:089b778962c4 38
Kruskal 8:089b778962c4 39 float average_angle=0;
Kruskal 8:089b778962c4 40 for(int i=25;i<50;i++){
Kruskal 8:089b778962c4 41 average_angle=average_angle+angle_buffer[i];
Kruskal 8:089b778962c4 42 }
Kruskal 8:089b778962c4 43 average_angle = average_angle/25;
Kruskal 8:089b778962c4 44 float speed_error = average_angle - 0.073;
Kruskal 8:089b778962c4 45 int Km_speed = 0;;
Kruskal 8:089b778962c4 46 if(speed_error<0)
Kruskal 8:089b778962c4 47 speed_error = speed_error*(-1);
Kruskal 8:089b778962c4 48
Kruskal 8:089b778962c4 49 if(speed_error <= 0.004 || speed_error >= 0.01){
Kruskal 8:089b778962c4 50 Km_speed = -13;
Kruskal 8:089b778962c4 51 }
Kruskal 8:089b778962c4 52 else{
Kruskal 8:089b778962c4 53 Km_speed = -7;
Kruskal 8:089b778962c4 54 }
Kruskal 8:089b778962c4 55
Kruskal 8:089b778962c4 56 float speed = Km_speed*speed_error;
Kruskal 8:089b778962c4 57 int int_max = pot1.read()*10;
Kruskal 8:089b778962c4 58 float max = int_max/(10.0);
Kruskal 8:089b778962c4 59 if(max+speed<0)
Kruskal 8:089b778962c4 60 speed = 0.2-max;
Kruskal 8:089b778962c4 61 motorA.rotate(max+speed);
Kruskal 8:089b778962c4 62 motorB.rotate(max+speed);
Kruskal 8:089b778962c4 63
Kruskal 8:089b778962c4 64 //pc.printf("\r\n Turn Angle = %f \r\n",turnAngle );
Kruskal 8:089b778962c4 65 servo.set_angle(turnAngle);
Kruskal 8:089b778962c4 66 Thread :: wait(30);
Kruskal 8:089b778962c4 67 }
Kruskal 8:089b778962c4 68 }
Kruskal 8:089b778962c4 69
Kruskal 8:089b778962c4 70 float find_error(){
Kruskal 7:fe8665daf3e7 71 cam.read();
Kruskal 8:089b778962c4 72 int left_target_point = 0;
Kruskal 8:089b778962c4 73 int left_average_point = 0;
Kruskal 8:089b778962c4 74 int left_black_count = 0;
Kruskal 8:089b778962c4 75 bool isLeftAllWhite = true;
Kruskal 7:fe8665daf3e7 76 for(int i=117;i>=10;i--){
Kruskal 8:089b778962c4 77 if(cam.sign_line_imageL[i] == ' '){
Kruskal 8:089b778962c4 78 left_black_count++;
Kruskal 8:089b778962c4 79 if(left_black_count == 10){
Kruskal 8:089b778962c4 80 left_target_point = i;
Kruskal 8:089b778962c4 81 isLeftAllWhite = false;
Kruskal 8:089b778962c4 82 break;
Kruskal 8:089b778962c4 83 }
Kruskal 8:089b778962c4 84 }
Kruskal 8:089b778962c4 85 else{
Kruskal 8:089b778962c4 86 left_black_count = 0;
Kruskal 8:089b778962c4 87 }
Kruskal 7:fe8665daf3e7 88 }
Kruskal 7:fe8665daf3e7 89
Kruskal 7:fe8665daf3e7 90 if(isLeftAllWhite){
Kruskal 7:fe8665daf3e7 91 //do not update the data
Kruskal 8:089b778962c4 92 //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5])/buffer_size;
Kruskal 8:089b778962c4 93 left_average_point = left_point_buffer[5];
Kruskal 7:fe8665daf3e7 94 }
Kruskal 7:fe8665daf3e7 95 else{
Kruskal 8:089b778962c4 96 //average the history data
Kruskal 8:089b778962c4 97 //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5]+ left_target_point)/(buffer_size+1);
Kruskal 8:089b778962c4 98 left_average_point = left_target_point;
Kruskal 7:fe8665daf3e7 99 }
backman 1:82bc25a7b68b 100
backman 0:68c173249c01 101
Kruskal 7:fe8665daf3e7 102 //====== find right line's left edging ======
Kruskal 8:089b778962c4 103 int right_target_point = 0;
Kruskal 7:fe8665daf3e7 104 int right_average_point = 0;
Kruskal 8:089b778962c4 105 int right_black_count = 0;
Kruskal 7:fe8665daf3e7 106 bool isRightAllWhite = true;
Kruskal 8:089b778962c4 107 for(int i=10;i<118;i++){
Kruskal 8:089b778962c4 108 if(cam.sign_line_imageR[i] == ' '){
Kruskal 7:fe8665daf3e7 109 right_black_count++;
Kruskal 8:089b778962c4 110 if(right_black_count == 10){
Kruskal 8:089b778962c4 111 right_target_point = i;
Kruskal 7:fe8665daf3e7 112 isRightAllWhite = false;
Kruskal 8:089b778962c4 113 break;
Kruskal 8:089b778962c4 114 }
Kruskal 8:089b778962c4 115 }
Kruskal 8:089b778962c4 116 else{
Kruskal 8:089b778962c4 117 right_black_count = 0;
Kruskal 7:fe8665daf3e7 118 }
Kruskal 7:fe8665daf3e7 119 }
Kruskal 8:089b778962c4 120
Kruskal 8:089b778962c4 121 /*for(int i=118;i>=10;i--){
Kruskal 8:089b778962c4 122 if(i<left_average_point){
Kruskal 8:089b778962c4 123 pc.printf(" ");
Kruskal 8:089b778962c4 124 }
Kruskal 8:089b778962c4 125 else
Kruskal 8:089b778962c4 126 pc.printf("O");
Kruskal 8:089b778962c4 127 }*/
Kruskal 8:089b778962c4 128
Kruskal 8:089b778962c4 129
Kruskal 8:089b778962c4 130
Kruskal 7:fe8665daf3e7 131
Kruskal 7:fe8665daf3e7 132 if(isRightAllWhite){
Kruskal 7:fe8665daf3e7 133 //do not update the buffer
Kruskal 8:089b778962c4 134 //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5])/buffer_size;
Kruskal 8:089b778962c4 135 right_average_point = right_point_buffer[5];
Kruskal 7:fe8665daf3e7 136 }
Kruskal 7:fe8665daf3e7 137 else{
Kruskal 7:fe8665daf3e7 138 //average the history data
Kruskal 8:089b778962c4 139 //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5] + right_target_point)/(buffer_size+1);
Kruskal 7:fe8665daf3e7 140 right_average_point = right_target_point;
Kruskal 8:089b778962c4 141 }
Kruskal 8:089b778962c4 142 /*
Kruskal 8:089b778962c4 143 for(int i=10;i<118;i++){
Kruskal 8:089b778962c4 144 if(i>=right_average_point){
Kruskal 8:089b778962c4 145 pc.printf(" ");
Kruskal 7:fe8665daf3e7 146 }
Kruskal 7:fe8665daf3e7 147 else
Kruskal 8:089b778962c4 148 pc.printf("O");
Kruskal 8:089b778962c4 149 }
Kruskal 8:089b778962c4 150 pc.printf("\r\n");*/
Kruskal 8:089b778962c4 151 //update buffer
Kruskal 8:089b778962c4 152 for(int i=0;i<buffer_size-1;i++){
Kruskal 8:089b778962c4 153 right_point_buffer[i] = right_point_buffer[i+1];
Kruskal 8:089b778962c4 154 left_point_buffer[i] = left_point_buffer[i+1];
Kruskal 7:fe8665daf3e7 155 }
Kruskal 7:fe8665daf3e7 156
Kruskal 8:089b778962c4 157 left_point_buffer[buffer_size-1] = left_average_point;
Kruskal 8:089b778962c4 158 right_point_buffer[buffer_size-1] = right_average_point;
backman 0:68c173249c01 159
Kruskal 8:089b778962c4 160 int center = 0;
Kruskal 8:089b778962c4 161 int reference_point = 0;
Kruskal 7:fe8665daf3e7 162 if(isLeftAllWhite && !isRightAllWhite){
Kruskal 8:089b778962c4 163 state = 1;
Kruskal 8:089b778962c4 164 Kp = 0.0004;
Kruskal 8:089b778962c4 165 center = 102;
Kruskal 7:fe8665daf3e7 166 reference_point = right_average_point;
Kruskal 8:089b778962c4 167
Kruskal 7:fe8665daf3e7 168 }
Kruskal 7:fe8665daf3e7 169 else if(!isLeftAllWhite && isRightAllWhite){
Kruskal 8:089b778962c4 170 state = 2;
Kruskal 8:089b778962c4 171 Kp = 0.0004;
Kruskal 8:089b778962c4 172 center = 20;
Kruskal 7:fe8665daf3e7 173 reference_point = left_average_point;
Kruskal 7:fe8665daf3e7 174 }
Kruskal 7:fe8665daf3e7 175 else if(isLeftAllWhite && isRightAllWhite){
Kruskal 8:089b778962c4 176 state = 0;
Kruskal 8:089b778962c4 177 Kp = 0.0004;
Kruskal 8:089b778962c4 178 center = 64;
Kruskal 8:089b778962c4 179 //straight line
Kruskal 8:089b778962c4 180 float average_angle;
Kruskal 8:089b778962c4 181 for(int i=0;i<50;i++){
Kruskal 8:089b778962c4 182 average_angle = average_angle + angle_buffer[i];
Kruskal 8:089b778962c4 183 }
Kruskal 8:089b778962c4 184 average_angle = average_angle/50;
Kruskal 7:fe8665daf3e7 185
Kruskal 8:089b778962c4 186 if(average_angle > 0.064 && average_angle < 0.08){
Kruskal 8:089b778962c4 187 Kp = 0;
Kruskal 8:089b778962c4 188 }
Kruskal 8:089b778962c4 189 else{
Kruskal 8:089b778962c4 190 if(left_average_point <= 40 && right_average_point <= 40)
Kruskal 8:089b778962c4 191 reference_point = 15;
Kruskal 8:089b778962c4 192 else if(left_average_point >= 70 && right_average_point >= 70)
Kruskal 8:089b778962c4 193 reference_point = 95;
Kruskal 8:089b778962c4 194 }
Kruskal 7:fe8665daf3e7 195 }
Kruskal 7:fe8665daf3e7 196 else if(!isLeftAllWhite && !isRightAllWhite){
Kruskal 8:089b778962c4 197 Kp = 0.0002;
Kruskal 8:089b778962c4 198
Kruskal 8:089b778962c4 199 if(128 - left_average_point >= right_average_point)
Kruskal 8:089b778962c4 200 {
Kruskal 8:089b778962c4 201 state = 1;
Kruskal 8:089b778962c4 202 center = 95;
Kruskal 8:089b778962c4 203 reference_point = right_average_point;
Kruskal 8:089b778962c4 204 }
Kruskal 8:089b778962c4 205 else{
Kruskal 8:089b778962c4 206 state = 2;
Kruskal 8:089b778962c4 207 center = 15;
Kruskal 8:089b778962c4 208 reference_point = left_average_point;
Kruskal 8:089b778962c4 209 }
Kruskal 8:089b778962c4 210 }
Kruskal 8:089b778962c4 211
Kruskal 8:089b778962c4 212
Kruskal 8:089b778962c4 213 return reference_point - center;
Kruskal 8:089b778962c4 214 }
Kruskal 8:089b778962c4 215
Kruskal 8:089b778962c4 216
Kruskal 8:089b778962c4 217 int main() {
Kruskal 8:089b778962c4 218
Kruskal 8:089b778962c4 219 while (1)
Kruskal 8:089b778962c4 220 {
Kruskal 8:089b778962c4 221 if (tsiSensor.readPercentage() > 0.00011)
Kruskal 8:089b778962c4 222 {
Kruskal 8:089b778962c4 223 Thread :: wait(2000);
Kruskal 8:089b778962c4 224 break;
Kruskal 8:089b778962c4 225 }
Kruskal 7:fe8665daf3e7 226 }
Kruskal 8:089b778962c4 227
Kruskal 8:089b778962c4 228 servo.set_angle(0.073);
Kruskal 8:089b778962c4 229
Kruskal 8:089b778962c4 230 #ifdef Debug_cam_uart
Kruskal 8:089b778962c4 231 pc.baud(115200);
Kruskal 8:089b778962c4 232
Kruskal 7:fe8665daf3e7 233
Kruskal 8:089b778962c4 234
Kruskal 8:089b778962c4 235
Kruskal 8:089b778962c4 236 motorA.rotate(0.33);
Kruskal 8:089b778962c4 237 motorB.rotate(0.33);
Kruskal 8:089b778962c4 238 Thread th_s(servo_thread);
Kruskal 8:089b778962c4 239 while(1){
Kruskal 8:089b778962c4 240 if(last_state != state){
Kruskal 8:089b778962c4 241 for(int i=0;i<e_buffer_size - 1;i++){
Kruskal 8:089b778962c4 242 error_buffer[i] = 0;
Kruskal 8:089b778962c4 243 }
Kruskal 8:089b778962c4 244 }
Kruskal 8:089b778962c4 245 else{
Kruskal 8:089b778962c4 246 for(int i=0;i<e_buffer_size - 1;i++){
Kruskal 8:089b778962c4 247 error_buffer[i] = error_buffer[i+1];
Kruskal 8:089b778962c4 248 }
Kruskal 8:089b778962c4 249 }
Kruskal 8:089b778962c4 250 error_buffer[e_buffer_size-1] = find_error();
Kruskal 7:fe8665daf3e7 251
Kruskal 8:089b778962c4 252 float past_error = 0;
Kruskal 8:089b778962c4 253 for(int i= 0;i<e_buffer_size;i++){
Kruskal 8:089b778962c4 254 past_error = (past_error*2)/3 + error_buffer[i];
Kruskal 8:089b778962c4 255 }
Kruskal 8:089b778962c4 256 past_error = past_error/e_buffer_size;
Kruskal 8:089b778962c4 257 float future_error = (error_buffer[e_buffer_size-1] - error_buffer[0])/e_buffer_size + error_buffer[e_buffer_size-1];
Kruskal 7:fe8665daf3e7 258
Kruskal 8:089b778962c4 259 stdio_mutex.lock();
Kruskal 8:089b778962c4 260 turnAngle = 0.073 + Kp*(error_buffer[e_buffer_size-1]
Kruskal 8:089b778962c4 261 + (1*past_error)/9
Kruskal 8:089b778962c4 262 + (4*future_error)/9);
Kruskal 8:089b778962c4 263 stdio_mutex.unlock();
Kruskal 8:089b778962c4 264 last_state = state;
Kruskal 8:089b778962c4 265
Kruskal 8:089b778962c4 266
Kruskal 8:089b778962c4 267 // pc.printf("%f \r\n",pot1.read());
Kruskal 8:089b778962c4 268
Kruskal 8:089b778962c4 269
Kruskal 8:089b778962c4 270 Thread :: wait(5);
Kruskal 8:089b778962c4 271 }
Kruskal 8:089b778962c4 272
Kruskal 7:fe8665daf3e7 273
Kruskal 7:fe8665daf3e7 274 #endif
Kruskal 7:fe8665daf3e7 275
Kruskal 7:fe8665daf3e7 276 return 0;
Kruskal 7:fe8665daf3e7 277
Kruskal 7:fe8665daf3e7 278 }