JuHwan Kim / Mbed OS 20161126_cmaker2

20161126_cmaker2

main.cpp@0:48c5db999587, 2016-12-08 (annotated)

Committer:
mcthemax
Date:
Thu Dec 08 07:31:10 2016 +0000
Revision:
0:48c5db999587
cmaker ton

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mcthemax 0:48c5db999587 1 #include "mbed.h"
mcthemax 0:48c5db999587 2
mcthemax 0:48c5db999587 3 LocalFileSystem local("local");
mcthemax 0:48c5db999587 4
mcthemax 0:48c5db999587 5 DigitalOut led1(LED1);
mcthemax 0:48c5db999587 6 DigitalOut led2(LED2);
mcthemax 0:48c5db999587 7 DigitalOut led3(LED3);
mcthemax 0:48c5db999587 8 DigitalOut led4(LED4);
mcthemax 0:48c5db999587 9
mcthemax 0:48c5db999587 10 /////////////////////////motor
mcthemax 0:48c5db999587 11 PwmOut rm_pwm(p21);
mcthemax 0:48c5db999587 12 DigitalOut rm_in1(p5);
mcthemax 0:48c5db999587 13 DigitalOut rm_in2(p6);
mcthemax 0:48c5db999587 14 DigitalOut rm_en(p7);
mcthemax 0:48c5db999587 15
mcthemax 0:48c5db999587 16 PwmOut lm_pwm(p22);
mcthemax 0:48c5db999587 17 DigitalOut lm_in1(p26);
mcthemax 0:48c5db999587 18 DigitalOut lm_in2(p25);
mcthemax 0:48c5db999587 19 DigitalOut lm_en(p24);
mcthemax 0:48c5db999587 20
mcthemax 0:48c5db999587 21 //0 : stop, 1: forward, 2: backward, 3: leftturn, 4: rightturn
mcthemax 0:48c5db999587 22 unsigned char moving = 0;
mcthemax 0:48c5db999587 23 unsigned char moving_command = '2';
mcthemax 0:48c5db999587 24 unsigned char pre_moving = 0;
mcthemax 0:48c5db999587 25
mcthemax 0:48c5db999587 26 ///////////////////////////
mcthemax 0:48c5db999587 27
mcthemax 0:48c5db999587 28 ///////////////////////////com
mcthemax 0:48c5db999587 29 Serial pc(USBTX, USBRX);
mcthemax 0:48c5db999587 30
mcthemax 0:48c5db999587 31 Serial mx(p28,p27);
mcthemax 0:48c5db999587 32
mcthemax 0:48c5db999587 33 DigitalOut water(p14);
mcthemax 0:48c5db999587 34 unsigned char demostate = 0;
mcthemax 0:48c5db999587 35 unsigned char fire_x = 0;
mcthemax 0:48c5db999587 36 unsigned char fire_y = 0;
mcthemax 0:48c5db999587 37 unsigned char moving_state = 0;
mcthemax 0:48c5db999587 38 unsigned char i, i2, i3, i4;
mcthemax 0:48c5db999587 39 int target_x = 0, target_y = 0;
mcthemax 0:48c5db999587 40 int error_x = 0, error_y = 0;
mcthemax 0:48c5db999587 41 int pre_error_x = 0, pre_error_y = 0;
mcthemax 0:48c5db999587 42 unsigned char fire_camera;
mcthemax 0:48c5db999587 43 unsigned char demo = 1;
mcthemax 0:48c5db999587 44 int dy_x = 2048;
mcthemax 0:48c5db999587 45 int dy_y = 1748;
mcthemax 0:48c5db999587 46
mcthemax 0:48c5db999587 47 unsigned char k;
mcthemax 0:48c5db999587 48 //depth_value = depth.read();
mcthemax 0:48c5db999587 49
mcthemax 0:48c5db999587 50 ///////////////////////////sense
mcthemax 0:48c5db999587 51 AnalogIn distance(p20);
mcthemax 0:48c5db999587 52 AnalogIn gas1(p19);
mcthemax 0:48c5db999587 53 AnalogIn gas2(p18);
mcthemax 0:48c5db999587 54 AnalogIn fire(p17);
mcthemax 0:48c5db999587 55 AnalogIn distance_l(p15);
mcthemax 0:48c5db999587 56 AnalogIn distance_r(p16);
mcthemax 0:48c5db999587 57
mcthemax 0:48c5db999587 58 Serial mysenl(p9, p10);
mcthemax 0:48c5db999587 59
mcthemax 0:48c5db999587 60 float distance_value = 0, gas1_value = 0, gas2_value = 0, fire_value = 0;
mcthemax 0:48c5db999587 61 float distance_r_value = 0;
mcthemax 0:48c5db999587 62 float distance_l_value = 0;
mcthemax 0:48c5db999587 63
mcthemax 0:48c5db999587 64 int roll, pitch, yaw, temp, check, r_check;
mcthemax 0:48c5db999587 65 int r,p,y;
mcthemax 0:48c5db999587 66 unsigned char input, input2, startb;
mcthemax 0:48c5db999587 67 /////////////////////////
mcthemax 0:48c5db999587 68
mcthemax 0:48c5db999587 69 ///////////////////////////////////////////////////
mcthemax 0:48c5db999587 70 void mx_write(unsigned char address, int data);
mcthemax 0:48c5db999587 71 void bulk_write(int speed1, int position1, int speed2, int position2);
mcthemax 0:48c5db999587 72 //////////////////////////////////
mcthemax 0:48c5db999587 73 void motor_thread(void const *args)
mcthemax 0:48c5db999587 74 {
mcthemax 0:48c5db999587 75 rm_pwm = 0.5;
mcthemax 0:48c5db999587 76 lm_pwm = 0.5;
mcthemax 0:48c5db999587 77
mcthemax 0:48c5db999587 78 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 79 rm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 80
mcthemax 0:48c5db999587 81 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 82 lm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 83
mcthemax 0:48c5db999587 84 rm_in1 = 0;
mcthemax 0:48c5db999587 85 rm_in2 = 0;
mcthemax 0:48c5db999587 86 rm_en = 0;
mcthemax 0:48c5db999587 87
mcthemax 0:48c5db999587 88 lm_in1 = 0;
mcthemax 0:48c5db999587 89 lm_in2 = 0;
mcthemax 0:48c5db999587 90 lm_en = 0;
mcthemax 0:48c5db999587 91
mcthemax 0:48c5db999587 92 while (true) {
mcthemax 0:48c5db999587 93 if(pre_moving!=moving)
mcthemax 0:48c5db999587 94 {
mcthemax 0:48c5db999587 95 if(moving==0)
mcthemax 0:48c5db999587 96 {
mcthemax 0:48c5db999587 97
mcthemax 0:48c5db999587 98 rm_in1 = 0;
mcthemax 0:48c5db999587 99 rm_in2 = 0;
mcthemax 0:48c5db999587 100 rm_en = 0;
mcthemax 0:48c5db999587 101
mcthemax 0:48c5db999587 102 lm_in1 = 0;
mcthemax 0:48c5db999587 103 lm_in2 = 0;
mcthemax 0:48c5db999587 104 lm_en = 0;
mcthemax 0:48c5db999587 105 }
mcthemax 0:48c5db999587 106 else if(moving==1)
mcthemax 0:48c5db999587 107 {
mcthemax 0:48c5db999587 108 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 109 rm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 110
mcthemax 0:48c5db999587 111 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 112 lm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 113 rm_in1 = 0;
mcthemax 0:48c5db999587 114 rm_in2 = 1;
mcthemax 0:48c5db999587 115 rm_en = 1;
mcthemax 0:48c5db999587 116
mcthemax 0:48c5db999587 117 lm_in1 = 1;
mcthemax 0:48c5db999587 118 lm_in2 = 0;
mcthemax 0:48c5db999587 119 lm_en = 1;
mcthemax 0:48c5db999587 120 }
mcthemax 0:48c5db999587 121 else if(moving==2)
mcthemax 0:48c5db999587 122 {
mcthemax 0:48c5db999587 123 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 124 rm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 125
mcthemax 0:48c5db999587 126 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 127 lm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 128 rm_in1 = 1;
mcthemax 0:48c5db999587 129 rm_in2 = 0;
mcthemax 0:48c5db999587 130 rm_en = 1;
mcthemax 0:48c5db999587 131
mcthemax 0:48c5db999587 132 lm_in1 = 0;
mcthemax 0:48c5db999587 133 lm_in2 = 1;
mcthemax 0:48c5db999587 134 lm_en = 1;
mcthemax 0:48c5db999587 135 }
mcthemax 0:48c5db999587 136 else if(moving==3)
mcthemax 0:48c5db999587 137 {
mcthemax 0:48c5db999587 138 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 139 rm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 140
mcthemax 0:48c5db999587 141 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 142 lm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 143 rm_in1 = 0;
mcthemax 0:48c5db999587 144 rm_in2 = 1;
mcthemax 0:48c5db999587 145 rm_en = 1;
mcthemax 0:48c5db999587 146
mcthemax 0:48c5db999587 147 lm_in1 = 0;
mcthemax 0:48c5db999587 148 lm_in2 = 1;
mcthemax 0:48c5db999587 149 lm_en = 1;
mcthemax 0:48c5db999587 150 }
mcthemax 0:48c5db999587 151 else if(moving==4)
mcthemax 0:48c5db999587 152 {
mcthemax 0:48c5db999587 153 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 154 rm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 155
mcthemax 0:48c5db999587 156 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 157 lm_pwm.pulsewidth_ms(10);
mcthemax 0:48c5db999587 158 rm_in1 = 1;
mcthemax 0:48c5db999587 159 rm_in2 = 0;
mcthemax 0:48c5db999587 160 rm_en = 1;
mcthemax 0:48c5db999587 161
mcthemax 0:48c5db999587 162 lm_in1 = 1;
mcthemax 0:48c5db999587 163 lm_in2 = 0;
mcthemax 0:48c5db999587 164 lm_en = 1;
mcthemax 0:48c5db999587 165 }
mcthemax 0:48c5db999587 166 else if(moving==5)
mcthemax 0:48c5db999587 167 {
mcthemax 0:48c5db999587 168 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 169 rm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 170
mcthemax 0:48c5db999587 171 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 172 lm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 173 rm_in1 = 0;
mcthemax 0:48c5db999587 174 rm_in2 = 1;
mcthemax 0:48c5db999587 175 rm_en = 1;
mcthemax 0:48c5db999587 176
mcthemax 0:48c5db999587 177 lm_in1 = 1;
mcthemax 0:48c5db999587 178 lm_in2 = 0;
mcthemax 0:48c5db999587 179 lm_en = 1;
mcthemax 0:48c5db999587 180 }
mcthemax 0:48c5db999587 181 else if(moving==7)
mcthemax 0:48c5db999587 182 {
mcthemax 0:48c5db999587 183 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 184 rm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 185
mcthemax 0:48c5db999587 186 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 187 lm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 188 rm_in1 = 0;
mcthemax 0:48c5db999587 189 rm_in2 = 1;
mcthemax 0:48c5db999587 190 rm_en = 1;
mcthemax 0:48c5db999587 191
mcthemax 0:48c5db999587 192 lm_in1 = 0;
mcthemax 0:48c5db999587 193 lm_in2 = 1;
mcthemax 0:48c5db999587 194 lm_en = 1;
mcthemax 0:48c5db999587 195 }
mcthemax 0:48c5db999587 196 else if(moving==6)
mcthemax 0:48c5db999587 197 {
mcthemax 0:48c5db999587 198 rm_pwm.period_ms(20);
mcthemax 0:48c5db999587 199 rm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 200
mcthemax 0:48c5db999587 201 lm_pwm.period_ms(20);
mcthemax 0:48c5db999587 202 lm_pwm.pulsewidth_ms(7);
mcthemax 0:48c5db999587 203 rm_in1 = 1;
mcthemax 0:48c5db999587 204 rm_in2 = 0;
mcthemax 0:48c5db999587 205 rm_en = 1;
mcthemax 0:48c5db999587 206
mcthemax 0:48c5db999587 207 lm_in1 = 1;
mcthemax 0:48c5db999587 208 lm_in2 = 0;
mcthemax 0:48c5db999587 209 lm_en = 1;
mcthemax 0:48c5db999587 210 }
mcthemax 0:48c5db999587 211 }
mcthemax 0:48c5db999587 212 //pc.printf("%d %d %d %d %d %d %d\r\n",moving, rm_in1.read(), rm_in2.read(), rm_en.read(), lm_in1.read(), lm_in2.read(), lm_en.read());
mcthemax 0:48c5db999587 213 pre_moving = moving;
mcthemax 0:48c5db999587 214
mcthemax 0:48c5db999587 215 led2 = !led2;
mcthemax 0:48c5db999587 216 Thread::wait(500);
mcthemax 0:48c5db999587 217 }
mcthemax 0:48c5db999587 218 }
mcthemax 0:48c5db999587 219 //////////////////////////////////
mcthemax 0:48c5db999587 220 void com_thread(void const *args)
mcthemax 0:48c5db999587 221 {
mcthemax 0:48c5db999587 222 bulk_write(5, 2048, 5, 2048);
mcthemax 0:48c5db999587 223 while (true) {
mcthemax 0:48c5db999587 224 if(pc.readable())
mcthemax 0:48c5db999587 225 {
mcthemax 0:48c5db999587 226 i = pc.getc();
mcthemax 0:48c5db999587 227
mcthemax 0:48c5db999587 228 if(i==100)
mcthemax 0:48c5db999587 229 {
mcthemax 0:48c5db999587 230 i = pc.getc();
mcthemax 0:48c5db999587 231 i = pc.getc();
mcthemax 0:48c5db999587 232 i2 = pc.getc();
mcthemax 0:48c5db999587 233 i3 = pc.getc();
mcthemax 0:48c5db999587 234 i4 = pc.getc();
mcthemax 0:48c5db999587 235
mcthemax 0:48c5db999587 236 fire_camera = pc.getc();
mcthemax 0:48c5db999587 237 moving_command = pc.getc();
mcthemax 0:48c5db999587 238 pc.printf("%d\r\n%f\r\n%d\r\n%d\r\n%d\r\n%d\r\n",temp,gas2_value,r,p,y,fire_camera);
mcthemax 0:48c5db999587 239
mcthemax 0:48c5db999587 240 target_x = (int)i*256+i2;
mcthemax 0:48c5db999587 241 target_y = (int)i3*256+i4;
mcthemax 0:48c5db999587 242 }
mcthemax 0:48c5db999587 243 if(fire_camera == 1)
mcthemax 0:48c5db999587 244 {
mcthemax 0:48c5db999587 245 error_x = 320-target_x;
mcthemax 0:48c5db999587 246 error_y = 240-target_y;
mcthemax 0:48c5db999587 247
mcthemax 0:48c5db999587 248 dy_x += error_x*0.1+(pre_error_x-error_x)*0.02;
mcthemax 0:48c5db999587 249 dy_y += error_y*0.1+(pre_error_y-error_y)*0.02;
mcthemax 0:48c5db999587 250
mcthemax 0:48c5db999587 251 pre_error_x = error_x;
mcthemax 0:48c5db999587 252 pre_error_y = error_y;
mcthemax 0:48c5db999587 253
mcthemax 0:48c5db999587 254 bulk_write(25, dy_x, 25, dy_y);
mcthemax 0:48c5db999587 255 }
mcthemax 0:48c5db999587 256 else
mcthemax 0:48c5db999587 257 {
mcthemax 0:48c5db999587 258 //dy_x = 2048;
mcthemax 0:48c5db999587 259 //dy_y = 1748;
mcthemax 0:48c5db999587 260 //bulk_write(10, dy_x, 10, dy_y);
mcthemax 0:48c5db999587 261 }
mcthemax 0:48c5db999587 262
mcthemax 0:48c5db999587 263 }
mcthemax 0:48c5db999587 264
mcthemax 0:48c5db999587 265
mcthemax 0:48c5db999587 266
mcthemax 0:48c5db999587 267 led3 = !led3;
mcthemax 0:48c5db999587 268 Thread::wait(10);
mcthemax 0:48c5db999587 269 }
mcthemax 0:48c5db999587 270 }
mcthemax 0:48c5db999587 271 //////////////////////////////////
mcthemax 0:48c5db999587 272 void sense_thread(void const *args)
mcthemax 0:48c5db999587 273 {
mcthemax 0:48c5db999587 274
mcthemax 0:48c5db999587 275 while (true) {
mcthemax 0:48c5db999587 276 if(mysenl.readable())
mcthemax 0:48c5db999587 277 {
mcthemax 0:48c5db999587 278 startb = mysenl.getc();
mcthemax 0:48c5db999587 279 }
mcthemax 0:48c5db999587 280 if(startb==0x02)
mcthemax 0:48c5db999587 281 {
mcthemax 0:48c5db999587 282 r_check = 0;
mcthemax 0:48c5db999587 283 input = mysenl.getc();
mcthemax 0:48c5db999587 284 input2 = mysenl.getc();
mcthemax 0:48c5db999587 285 r_check+=input;
mcthemax 0:48c5db999587 286 r_check+=input2;
mcthemax 0:48c5db999587 287 roll = (input*256+input2)/100-180;
mcthemax 0:48c5db999587 288
mcthemax 0:48c5db999587 289 input = mysenl.getc();
mcthemax 0:48c5db999587 290 input2 = mysenl.getc();
mcthemax 0:48c5db999587 291 r_check+=input;
mcthemax 0:48c5db999587 292 r_check+=input2;
mcthemax 0:48c5db999587 293 pitch = (input*256+input2)/100-90;
mcthemax 0:48c5db999587 294
mcthemax 0:48c5db999587 295 input = mysenl.getc();
mcthemax 0:48c5db999587 296 input2 = mysenl.getc();
mcthemax 0:48c5db999587 297 r_check+=input;
mcthemax 0:48c5db999587 298 r_check+=input2;
mcthemax 0:48c5db999587 299 yaw = (input*256+input2)/100-180;
mcthemax 0:48c5db999587 300
mcthemax 0:48c5db999587 301 input = mysenl.getc();
mcthemax 0:48c5db999587 302 input2 = mysenl.getc();
mcthemax 0:48c5db999587 303
mcthemax 0:48c5db999587 304 temp = (input*256+input2)/100-100;
mcthemax 0:48c5db999587 305 r_check+=input;
mcthemax 0:48c5db999587 306 r_check+=input2;
mcthemax 0:48c5db999587 307
mcthemax 0:48c5db999587 308
mcthemax 0:48c5db999587 309 input = mysenl.getc();
mcthemax 0:48c5db999587 310 input2 = mysenl.getc();
mcthemax 0:48c5db999587 311
mcthemax 0:48c5db999587 312 input = mysenl.getc();
mcthemax 0:48c5db999587 313 input2 = mysenl.getc();
mcthemax 0:48c5db999587 314 if(input==44&&input2==3)
mcthemax 0:48c5db999587 315 {
mcthemax 0:48c5db999587 316 r=roll;
mcthemax 0:48c5db999587 317 p=pitch;
mcthemax 0:48c5db999587 318 y=yaw;
mcthemax 0:48c5db999587 319 distance_value = distance.read();
mcthemax 0:48c5db999587 320 distance_r_value = distance_r.read();
mcthemax 0:48c5db999587 321 distance_l_value = distance_l.read();
mcthemax 0:48c5db999587 322 gas1_value = gas1.read();
mcthemax 0:48c5db999587 323 gas2_value = gas2.read();
mcthemax 0:48c5db999587 324 fire_value = fire.read();
mcthemax 0:48c5db999587 325 // pc.printf("\t%d\t%d\t%d\n\r\t%d\t%d\t%d\t%d\t%d\r\n",r, p, y, temp, r_check,check, input, input2);
mcthemax 0:48c5db999587 326 //pc.printf("\tdis%f\tdis_r%f\tdis_l%f\r\n\tgas %f\tgas %f\tfire %f\r\n",distance_value,distance_r_value,distance_l_value, gas1_value, gas2_value, fire_value);
mcthemax 0:48c5db999587 327 }
mcthemax 0:48c5db999587 328 }
mcthemax 0:48c5db999587 329 distance_value = distance.read();
mcthemax 0:48c5db999587 330 distance_r_value = distance_r.read();
mcthemax 0:48c5db999587 331 distance_l_value = distance_l.read();
mcthemax 0:48c5db999587 332 gas1_value = gas1.read();
mcthemax 0:48c5db999587 333 gas2_value = gas2.read();
mcthemax 0:48c5db999587 334 fire_value = fire.read();
mcthemax 0:48c5db999587 335 if(demo==1)
mcthemax 0:48c5db999587 336 {
mcthemax 0:48c5db999587 337 if(fire_camera==0&&demostate==0)
mcthemax 0:48c5db999587 338 {
mcthemax 0:48c5db999587 339 bulk_write(5, 2048, 5, 2048);
mcthemax 0:48c5db999587 340 moving = 1;
mcthemax 0:48c5db999587 341 Thread::wait(1000);
mcthemax 0:48c5db999587 342 moving = 2;
mcthemax 0:48c5db999587 343 Thread::wait(1000);
mcthemax 0:48c5db999587 344
mcthemax 0:48c5db999587 345 }
mcthemax 0:48c5db999587 346 else if((distance_r_value>0.5||distance_l_value>0.5||distance_value>0.2)&&fire_camera&&demostate==1)
mcthemax 0:48c5db999587 347 {
mcthemax 0:48c5db999587 348 moving = 0;
mcthemax 0:48c5db999587 349 Thread::wait(2000);
mcthemax 0:48c5db999587 350 water = 1;
mcthemax 0:48c5db999587 351 Thread::wait(1300);
mcthemax 0:48c5db999587 352 water = 0;
mcthemax 0:48c5db999587 353 demostate = 0;
mcthemax 0:48c5db999587 354 }
mcthemax 0:48c5db999587 355 else if(fire_camera==1)
mcthemax 0:48c5db999587 356 {
mcthemax 0:48c5db999587 357 demostate = 1;
mcthemax 0:48c5db999587 358 if(dy_x>2248)
mcthemax 0:48c5db999587 359 {
mcthemax 0:48c5db999587 360 moving = 7;
mcthemax 0:48c5db999587 361 Thread::wait(100);
mcthemax 0:48c5db999587 362 }
mcthemax 0:48c5db999587 363 else if(dy_x<1748)
mcthemax 0:48c5db999587 364 {
mcthemax 0:48c5db999587 365 moving = 6;
mcthemax 0:48c5db999587 366 Thread::wait(100);
mcthemax 0:48c5db999587 367
mcthemax 0:48c5db999587 368 }
mcthemax 0:48c5db999587 369 moving = 5;
mcthemax 0:48c5db999587 370
mcthemax 0:48c5db999587 371 }
mcthemax 0:48c5db999587 372
mcthemax 0:48c5db999587 373 }
mcthemax 0:48c5db999587 374 else
mcthemax 0:48c5db999587 375 {
mcthemax 0:48c5db999587 376 if(moving_command==1)
mcthemax 0:48c5db999587 377 {
mcthemax 0:48c5db999587 378 if(moving_state==0)
mcthemax 0:48c5db999587 379 {
mcthemax 0:48c5db999587 380 moving = 2;
mcthemax 0:48c5db999587 381
mcthemax 0:48c5db999587 382 if(fire_camera==1)
mcthemax 0:48c5db999587 383 moving_state = 1;
mcthemax 0:48c5db999587 384 }
mcthemax 0:48c5db999587 385 else if(moving_state==1)
mcthemax 0:48c5db999587 386 {
mcthemax 0:48c5db999587 387 moving_state = 1;
mcthemax 0:48c5db999587 388 //moving
mcthemax 0:48c5db999587 389 if(distance_r_value>0.7&&distance_l_value>0.7)
mcthemax 0:48c5db999587 390 moving = 2;
mcthemax 0:48c5db999587 391 else if(distance_r_value>0.7)
mcthemax 0:48c5db999587 392 moving = 3;
mcthemax 0:48c5db999587 393 else if(distance_l_value>0.7)
mcthemax 0:48c5db999587 394 moving = 4;
mcthemax 0:48c5db999587 395 else
mcthemax 0:48c5db999587 396 moving = 1;
mcthemax 0:48c5db999587 397
mcthemax 0:48c5db999587 398 if(dy_x>2148)
mcthemax 0:48c5db999587 399 {
mcthemax 0:48c5db999587 400 moving = 4;
mcthemax 0:48c5db999587 401 Thread::wait(500);
mcthemax 0:48c5db999587 402 }
mcthemax 0:48c5db999587 403 else if(dy_x<1948)
mcthemax 0:48c5db999587 404 {
mcthemax 0:48c5db999587 405 moving = 3;
mcthemax 0:48c5db999587 406 Thread::wait(500);
mcthemax 0:48c5db999587 407
mcthemax 0:48c5db999587 408 }
mcthemax 0:48c5db999587 409 if(distance_value>0.5)
mcthemax 0:48c5db999587 410 moving_state = 2;
mcthemax 0:48c5db999587 411 }
mcthemax 0:48c5db999587 412 else if(moving_state == 2);
mcthemax 0:48c5db999587 413 {
mcthemax 0:48c5db999587 414 Thread::wait(5000);
mcthemax 0:48c5db999587 415 water = 1;
mcthemax 0:48c5db999587 416 Thread::wait(1300);
mcthemax 0:48c5db999587 417 water = 0;
mcthemax 0:48c5db999587 418
mcthemax 0:48c5db999587 419 if(fire_camera==0)
mcthemax 0:48c5db999587 420 moving_state = 0;
mcthemax 0:48c5db999587 421 }
mcthemax 0:48c5db999587 422
mcthemax 0:48c5db999587 423 }
mcthemax 0:48c5db999587 424 else
mcthemax 0:48c5db999587 425 {
mcthemax 0:48c5db999587 426 // moving_state = 0;
mcthemax 0:48c5db999587 427 //bulk_write(5, 2048, 5, 2048);
mcthemax 0:48c5db999587 428 //water = 0;
mcthemax 0:48c5db999587 429 //moving = 0;
mcthemax 0:48c5db999587 430 //moving = moving_command;
mcthemax 0:48c5db999587 431 }
mcthemax 0:48c5db999587 432 }
mcthemax 0:48c5db999587 433 led4 = !led4;
mcthemax 0:48c5db999587 434 Thread::wait(50);
mcthemax 0:48c5db999587 435 }
mcthemax 0:48c5db999587 436 }
mcthemax 0:48c5db999587 437 // main() runs in its own thread in the OS
mcthemax 0:48c5db999587 438 // (note the calls to Thread::wait below for delays)
mcthemax 0:48c5db999587 439 int main()
mcthemax 0:48c5db999587 440 {
mcthemax 0:48c5db999587 441 pc.baud(57600);
mcthemax 0:48c5db999587 442 mx.baud(57600);
mcthemax 0:48c5db999587 443 mysenl.baud(115200);
mcthemax 0:48c5db999587 444
mcthemax 0:48c5db999587 445 Thread::wait(1000);
mcthemax 0:48c5db999587 446
mcthemax 0:48c5db999587 447 Thread thread1(motor_thread);
mcthemax 0:48c5db999587 448 Thread thread2(com_thread);
mcthemax 0:48c5db999587 449 Thread thread3(sense_thread);
mcthemax 0:48c5db999587 450
mcthemax 0:48c5db999587 451 while (true) {
mcthemax 0:48c5db999587 452
mcthemax 0:48c5db999587 453
mcthemax 0:48c5db999587 454
mcthemax 0:48c5db999587 455 /*
mcthemax 0:48c5db999587 456
mcthemax 0:48c5db999587 457 if(pc.readable())
mcthemax 0:48c5db999587 458 {
mcthemax 0:48c5db999587 459 i = pc.getc();
mcthemax 0:48c5db999587 460
mcthemax 0:48c5db999587 461
mcthemax 0:48c5db999587 462 if(i=='w')
mcthemax 0:48c5db999587 463 moving_command = 1;
mcthemax 0:48c5db999587 464 else if(i=='s')
mcthemax 0:48c5db999587 465 moving_command = 2;
mcthemax 0:48c5db999587 466 else if(i=='a')
mcthemax 0:48c5db999587 467 moving_command = 3;
mcthemax 0:48c5db999587 468 else if(i=='d')
mcthemax 0:48c5db999587 469 moving_command = 4;
mcthemax 0:48c5db999587 470 else if(i=='q')
mcthemax 0:48c5db999587 471 moving_command = 0;
mcthemax 0:48c5db999587 472 else if(i=='t')
mcthemax 0:48c5db999587 473 moving_command = 5;
mcthemax 0:48c5db999587 474
mcthemax 0:48c5db999587 475 else if(i=='z')
mcthemax 0:48c5db999587 476 dy_x += 10;
mcthemax 0:48c5db999587 477 else if(i=='x')
mcthemax 0:48c5db999587 478 dy_x -= 10;
mcthemax 0:48c5db999587 479 else if(i=='c')
mcthemax 0:48c5db999587 480 dy_y += 10;
mcthemax 0:48c5db999587 481 else if(i=='v')
mcthemax 0:48c5db999587 482 dy_y -= 10;
mcthemax 0:48c5db999587 483 else if(i=='e')
mcthemax 0:48c5db999587 484 water = 1;
mcthemax 0:48c5db999587 485 else if(i=='r')
mcthemax 0:48c5db999587 486 water = 0;
mcthemax 0:48c5db999587 487
mcthemax 0:48c5db999587 488 }
mcthemax 0:48c5db999587 489 */
mcthemax 0:48c5db999587 490 led1 = !led1;
mcthemax 0:48c5db999587 491 Thread::wait(500);
mcthemax 0:48c5db999587 492
mcthemax 0:48c5db999587 493 }
mcthemax 0:48c5db999587 494 }
mcthemax 0:48c5db999587 495
mcthemax 0:48c5db999587 496 void mx_write(unsigned char address, int data)
mcthemax 0:48c5db999587 497 {
mcthemax 0:48c5db999587 498
mcthemax 0:48c5db999587 499 unsigned char k = 0x07+0x05+0x03+address+((data&0xff00)>>8)+(data&0xff);
mcthemax 0:48c5db999587 500 k = 0xff-k;
mcthemax 0:48c5db999587 501
mcthemax 0:48c5db999587 502 mx.putc(0xff);
mcthemax 0:48c5db999587 503 mx.putc(0xff);
mcthemax 0:48c5db999587 504 mx.putc(0x07);
mcthemax 0:48c5db999587 505 mx.putc(0x05);
mcthemax 0:48c5db999587 506 mx.putc(0x03);
mcthemax 0:48c5db999587 507 mx.putc(address);
mcthemax 0:48c5db999587 508 mx.putc((unsigned char)(data&0xff));
mcthemax 0:48c5db999587 509 mx.putc((unsigned char)((data&0xff00)>>8));
mcthemax 0:48c5db999587 510 mx.putc(k);
mcthemax 0:48c5db999587 511
mcthemax 0:48c5db999587 512 pc.printf("%d %d %d %d %d %d %d %d %d\n\r",0xff,0xff,0x07,0x05,0x03,address,data&0xff,(data&0xff00)>>8,k);
mcthemax 0:48c5db999587 513 }
mcthemax 0:48c5db999587 514
mcthemax 0:48c5db999587 515 void bulk_write(int speed1, int position1, int speed2, int position2)
mcthemax 0:48c5db999587 516 {
mcthemax 0:48c5db999587 517 k = 0xfe;
mcthemax 0:48c5db999587 518 k += 0x0e;
mcthemax 0:48c5db999587 519 k += 0x83;
mcthemax 0:48c5db999587 520 k += 0x1e;
mcthemax 0:48c5db999587 521 k += 0x04;
mcthemax 0:48c5db999587 522 //k = 0xfe+0x0e+0x83+0x1e+0x04;
mcthemax 0:48c5db999587 523 k+=0x04+((position1&0xff00)>>8)+(position1&0xff)+((speed1&0xff00)>>8)+(speed1&0xff);
mcthemax 0:48c5db999587 524 k+=0x08+((position2&0xff00)>>8)+(position2&0xff)+((speed2&0xff00)>>8)+(speed2&0xff);
mcthemax 0:48c5db999587 525 k = 0xff-k;
mcthemax 0:48c5db999587 526
mcthemax 0:48c5db999587 527 mx.putc(0xff);
mcthemax 0:48c5db999587 528 mx.putc(0xff);
mcthemax 0:48c5db999587 529 mx.putc(0xfe);
mcthemax 0:48c5db999587 530 mx.putc(0x0e);
mcthemax 0:48c5db999587 531 mx.putc(0x83);
mcthemax 0:48c5db999587 532 mx.putc(0x1E);
mcthemax 0:48c5db999587 533 mx.putc(0x04);
mcthemax 0:48c5db999587 534 mx.putc(0x04);
mcthemax 0:48c5db999587 535 mx.putc((unsigned char)(position1&0xff));
mcthemax 0:48c5db999587 536 mx.putc((unsigned char)((position1&0xff00)>>8));
mcthemax 0:48c5db999587 537 mx.putc((unsigned char)(speed1&0xff));
mcthemax 0:48c5db999587 538 mx.putc((unsigned char)((speed1&0xff00)>>8));
mcthemax 0:48c5db999587 539 mx.putc(0x08);
mcthemax 0:48c5db999587 540 mx.putc((unsigned char)(position2&0xff));
mcthemax 0:48c5db999587 541 mx.putc((unsigned char)((position2&0xff00)>>8));
mcthemax 0:48c5db999587 542 mx.putc((unsigned char)(speed2&0xff));
mcthemax 0:48c5db999587 543 mx.putc((unsigned char)((speed2&0xff00)>>8));
mcthemax 0:48c5db999587 544 mx.putc(k);
mcthemax 0:48c5db999587 545 }