20161126_cmaker2
Diff: main.cpp
- Revision:
- 0:48c5db999587
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 08 07:31:10 2016 +0000
@@ -0,0 +1,545 @@
+#include "mbed.h"
+
+LocalFileSystem local("local");
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+/////////////////////////motor
+PwmOut rm_pwm(p21);
+DigitalOut rm_in1(p5);
+DigitalOut rm_in2(p6);
+DigitalOut rm_en(p7);
+
+PwmOut lm_pwm(p22);
+DigitalOut lm_in1(p26);
+DigitalOut lm_in2(p25);
+DigitalOut lm_en(p24);
+
+//0 : stop, 1: forward, 2: backward, 3: leftturn, 4: rightturn
+unsigned char moving = 0;
+unsigned char moving_command = '2';
+unsigned char pre_moving = 0;
+
+///////////////////////////
+
+///////////////////////////com
+Serial pc(USBTX, USBRX);
+
+Serial mx(p28,p27);
+
+DigitalOut water(p14);
+unsigned char demostate = 0;
+unsigned char fire_x = 0;
+unsigned char fire_y = 0;
+unsigned char moving_state = 0;
+unsigned char i, i2, i3, i4;
+int target_x = 0, target_y = 0;
+int error_x = 0, error_y = 0;
+int pre_error_x = 0, pre_error_y = 0;
+unsigned char fire_camera;
+unsigned char demo = 1;
+int dy_x = 2048;
+int dy_y = 1748;
+
+unsigned char k;
+//depth_value = depth.read();
+
+///////////////////////////sense
+AnalogIn distance(p20);
+AnalogIn gas1(p19);
+AnalogIn gas2(p18);
+AnalogIn fire(p17);
+AnalogIn distance_l(p15);
+AnalogIn distance_r(p16);
+
+Serial mysenl(p9, p10);
+
+float distance_value = 0, gas1_value = 0, gas2_value = 0, fire_value = 0;
+float distance_r_value = 0;
+float distance_l_value = 0;
+
+int roll, pitch, yaw, temp, check, r_check;
+int r,p,y;
+unsigned char input, input2, startb;
+/////////////////////////
+
+///////////////////////////////////////////////////
+void mx_write(unsigned char address, int data);
+void bulk_write(int speed1, int position1, int speed2, int position2);
+//////////////////////////////////
+void motor_thread(void const *args)
+{
+ rm_pwm = 0.5;
+ lm_pwm = 0.5;
+
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(10);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(10);
+
+ rm_in1 = 0;
+ rm_in2 = 0;
+ rm_en = 0;
+
+ lm_in1 = 0;
+ lm_in2 = 0;
+ lm_en = 0;
+
+ while (true) {
+ if(pre_moving!=moving)
+ {
+ if(moving==0)
+ {
+
+ rm_in1 = 0;
+ rm_in2 = 0;
+ rm_en = 0;
+
+ lm_in1 = 0;
+ lm_in2 = 0;
+ lm_en = 0;
+ }
+ else if(moving==1)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(10);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(10);
+ rm_in1 = 0;
+ rm_in2 = 1;
+ rm_en = 1;
+
+ lm_in1 = 1;
+ lm_in2 = 0;
+ lm_en = 1;
+ }
+ else if(moving==2)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(10);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(10);
+ rm_in1 = 1;
+ rm_in2 = 0;
+ rm_en = 1;
+
+ lm_in1 = 0;
+ lm_in2 = 1;
+ lm_en = 1;
+ }
+ else if(moving==3)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(10);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(10);
+ rm_in1 = 0;
+ rm_in2 = 1;
+ rm_en = 1;
+
+ lm_in1 = 0;
+ lm_in2 = 1;
+ lm_en = 1;
+ }
+ else if(moving==4)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(10);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(10);
+ rm_in1 = 1;
+ rm_in2 = 0;
+ rm_en = 1;
+
+ lm_in1 = 1;
+ lm_in2 = 0;
+ lm_en = 1;
+ }
+ else if(moving==5)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(7);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(7);
+ rm_in1 = 0;
+ rm_in2 = 1;
+ rm_en = 1;
+
+ lm_in1 = 1;
+ lm_in2 = 0;
+ lm_en = 1;
+ }
+ else if(moving==7)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(7);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(7);
+ rm_in1 = 0;
+ rm_in2 = 1;
+ rm_en = 1;
+
+ lm_in1 = 0;
+ lm_in2 = 1;
+ lm_en = 1;
+ }
+ else if(moving==6)
+ {
+ rm_pwm.period_ms(20);
+ rm_pwm.pulsewidth_ms(7);
+
+ lm_pwm.period_ms(20);
+ lm_pwm.pulsewidth_ms(7);
+ rm_in1 = 1;
+ rm_in2 = 0;
+ rm_en = 1;
+
+ lm_in1 = 1;
+ lm_in2 = 0;
+ lm_en = 1;
+ }
+ }
+ //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());
+ pre_moving = moving;
+
+ led2 = !led2;
+ Thread::wait(500);
+ }
+}
+//////////////////////////////////
+void com_thread(void const *args)
+{
+ bulk_write(5, 2048, 5, 2048);
+ while (true) {
+ if(pc.readable())
+ {
+ i = pc.getc();
+
+ if(i==100)
+ {
+ i = pc.getc();
+ i = pc.getc();
+ i2 = pc.getc();
+ i3 = pc.getc();
+ i4 = pc.getc();
+
+ fire_camera = pc.getc();
+ moving_command = pc.getc();
+ 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);
+
+ target_x = (int)i*256+i2;
+ target_y = (int)i3*256+i4;
+ }
+ if(fire_camera == 1)
+ {
+ error_x = 320-target_x;
+ error_y = 240-target_y;
+
+ dy_x += error_x*0.1+(pre_error_x-error_x)*0.02;
+ dy_y += error_y*0.1+(pre_error_y-error_y)*0.02;
+
+ pre_error_x = error_x;
+ pre_error_y = error_y;
+
+ bulk_write(25, dy_x, 25, dy_y);
+ }
+ else
+ {
+ //dy_x = 2048;
+ //dy_y = 1748;
+ //bulk_write(10, dy_x, 10, dy_y);
+ }
+
+ }
+
+
+
+ led3 = !led3;
+ Thread::wait(10);
+ }
+}
+//////////////////////////////////
+void sense_thread(void const *args)
+{
+
+ while (true) {
+ if(mysenl.readable())
+ {
+ startb = mysenl.getc();
+ }
+ if(startb==0x02)
+ {
+ r_check = 0;
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+ r_check+=input;
+ r_check+=input2;
+ roll = (input*256+input2)/100-180;
+
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+ r_check+=input;
+ r_check+=input2;
+ pitch = (input*256+input2)/100-90;
+
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+ r_check+=input;
+ r_check+=input2;
+ yaw = (input*256+input2)/100-180;
+
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+
+ temp = (input*256+input2)/100-100;
+ r_check+=input;
+ r_check+=input2;
+
+
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+
+ input = mysenl.getc();
+ input2 = mysenl.getc();
+ if(input==44&&input2==3)
+ {
+ r=roll;
+ p=pitch;
+ y=yaw;
+ distance_value = distance.read();
+ distance_r_value = distance_r.read();
+ distance_l_value = distance_l.read();
+ gas1_value = gas1.read();
+ gas2_value = gas2.read();
+ fire_value = fire.read();
+ // 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);
+ //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);
+ }
+ }
+ distance_value = distance.read();
+ distance_r_value = distance_r.read();
+ distance_l_value = distance_l.read();
+ gas1_value = gas1.read();
+ gas2_value = gas2.read();
+ fire_value = fire.read();
+ if(demo==1)
+ {
+ if(fire_camera==0&&demostate==0)
+ {
+ bulk_write(5, 2048, 5, 2048);
+ moving = 1;
+ Thread::wait(1000);
+ moving = 2;
+ Thread::wait(1000);
+
+ }
+ else if((distance_r_value>0.5||distance_l_value>0.5||distance_value>0.2)&&fire_camera&&demostate==1)
+ {
+ moving = 0;
+ Thread::wait(2000);
+ water = 1;
+ Thread::wait(1300);
+ water = 0;
+ demostate = 0;
+ }
+ else if(fire_camera==1)
+ {
+ demostate = 1;
+ if(dy_x>2248)
+ {
+ moving = 7;
+ Thread::wait(100);
+ }
+ else if(dy_x<1748)
+ {
+ moving = 6;
+ Thread::wait(100);
+
+ }
+ moving = 5;
+
+ }
+
+ }
+ else
+ {
+ if(moving_command==1)
+ {
+ if(moving_state==0)
+ {
+ moving = 2;
+
+ if(fire_camera==1)
+ moving_state = 1;
+ }
+ else if(moving_state==1)
+ {
+ moving_state = 1;
+ //moving
+ if(distance_r_value>0.7&&distance_l_value>0.7)
+ moving = 2;
+ else if(distance_r_value>0.7)
+ moving = 3;
+ else if(distance_l_value>0.7)
+ moving = 4;
+ else
+ moving = 1;
+
+ if(dy_x>2148)
+ {
+ moving = 4;
+ Thread::wait(500);
+ }
+ else if(dy_x<1948)
+ {
+ moving = 3;
+ Thread::wait(500);
+
+ }
+ if(distance_value>0.5)
+ moving_state = 2;
+ }
+ else if(moving_state == 2);
+ {
+ Thread::wait(5000);
+ water = 1;
+ Thread::wait(1300);
+ water = 0;
+
+ if(fire_camera==0)
+ moving_state = 0;
+ }
+
+ }
+ else
+ {
+ // moving_state = 0;
+ //bulk_write(5, 2048, 5, 2048);
+ //water = 0;
+ //moving = 0;
+ //moving = moving_command;
+ }
+ }
+ led4 = !led4;
+ Thread::wait(50);
+ }
+}
+// main() runs in its own thread in the OS
+// (note the calls to Thread::wait below for delays)
+int main()
+{
+ pc.baud(57600);
+ mx.baud(57600);
+ mysenl.baud(115200);
+
+ Thread::wait(1000);
+
+ Thread thread1(motor_thread);
+ Thread thread2(com_thread);
+ Thread thread3(sense_thread);
+
+ while (true) {
+
+
+
+ /*
+
+ if(pc.readable())
+ {
+ i = pc.getc();
+
+
+ if(i=='w')
+ moving_command = 1;
+ else if(i=='s')
+ moving_command = 2;
+ else if(i=='a')
+ moving_command = 3;
+ else if(i=='d')
+ moving_command = 4;
+ else if(i=='q')
+ moving_command = 0;
+ else if(i=='t')
+ moving_command = 5;
+
+ else if(i=='z')
+ dy_x += 10;
+ else if(i=='x')
+ dy_x -= 10;
+ else if(i=='c')
+ dy_y += 10;
+ else if(i=='v')
+ dy_y -= 10;
+ else if(i=='e')
+ water = 1;
+ else if(i=='r')
+ water = 0;
+
+ }
+ */
+ led1 = !led1;
+ Thread::wait(500);
+
+ }
+}
+
+void mx_write(unsigned char address, int data)
+{
+
+ unsigned char k = 0x07+0x05+0x03+address+((data&0xff00)>>8)+(data&0xff);
+ k = 0xff-k;
+
+ mx.putc(0xff);
+ mx.putc(0xff);
+ mx.putc(0x07);
+ mx.putc(0x05);
+ mx.putc(0x03);
+ mx.putc(address);
+ mx.putc((unsigned char)(data&0xff));
+ mx.putc((unsigned char)((data&0xff00)>>8));
+ mx.putc(k);
+
+ 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);
+}
+
+void bulk_write(int speed1, int position1, int speed2, int position2)
+{
+ k = 0xfe;
+ k += 0x0e;
+ k += 0x83;
+ k += 0x1e;
+ k += 0x04;
+ //k = 0xfe+0x0e+0x83+0x1e+0x04;
+ k+=0x04+((position1&0xff00)>>8)+(position1&0xff)+((speed1&0xff00)>>8)+(speed1&0xff);
+ k+=0x08+((position2&0xff00)>>8)+(position2&0xff)+((speed2&0xff00)>>8)+(speed2&0xff);
+ k = 0xff-k;
+
+ mx.putc(0xff);
+ mx.putc(0xff);
+ mx.putc(0xfe);
+ mx.putc(0x0e);
+ mx.putc(0x83);
+ mx.putc(0x1E);
+ mx.putc(0x04);
+ mx.putc(0x04);
+ mx.putc((unsigned char)(position1&0xff));
+ mx.putc((unsigned char)((position1&0xff00)>>8));
+ mx.putc((unsigned char)(speed1&0xff));
+ mx.putc((unsigned char)((speed1&0xff00)>>8));
+ mx.putc(0x08);
+ mx.putc((unsigned char)(position2&0xff));
+ mx.putc((unsigned char)((position2&0xff00)>>8));
+ mx.putc((unsigned char)(speed2&0xff));
+ mx.putc((unsigned char)((speed2&0xff00)>>8));
+ mx.putc(k);
+}
\ No newline at end of file