File content as of revision 0:48c5db999587:

#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);
}