File content as of revision 0:7f0b090897bc:

#include "mbed.h"

#define PWMMAX 0.33
#define stMAX 1023
#define stMIN 0
#define daMAX 255
#define SERVO_MIN_ANG 1020 //us 0.0

Serial comUSB(USBTX,USBRX);
Serial com1(p9,p10);
//Serial com1(p13, p14);

DigitalIn angMAX(p18);
DigitalIn angMIN(p19);
DigitalOut bufcheck(LED1);
DigitalOut dirR(p28);
DigitalOut dirL(p29);
DigitalOut dirA(p20);
DigitalOut shoter(p11);
DigitalOut loader(p12);
PwmOut servo(p24);
PwmOut pwrR(p26);
PwmOut pwrL(p25);
PwmOut pwrA(p21);

const float angpwr = 0.18;
int buf_count = -1;

union Get_data{
    char alldata[6];
    struct{
        char header :8;
        unsigned int x :8;
        unsigned int y :8;
        unsigned int t :8;
        int au :1;
        int ad :1;
        int h :1;
        int set :1;
        int s :1;
        int vx :1;
        int vy :1;
        int vt :1;
    }data;
}buf_data;

union union_data{
    char alldata[9];
    struct{
        char header :8;
        signed int x :16;
        signed int y :16;
        signed int t :16;
        int au :1;
        int ad :1;
        int h :1;
        int set :1;
        int s :1;
    }data;
}act_data;



void Serialsetup(){
    comUSB.baud(9600);
    com1.baud(115200);
}

void interface(){
    char data;
    bufcheck = 1;
    while(com1.readable()){
        data = com1.getc();
        bufcheck = 1;
        //comUSB.printf("%d\r\n",data);
        if(data == 0xFE){
            buf_count = 0;
            
        }
       if(buf_count < 6 && buf_count != -1){
            buf_data.alldata[buf_count] = data;
            buf_count++;
       }
       
    }
    
    if(buf_count == 6 && buf_data.alldata[0] == 0xFE){
        if(buf_data.data.vx == 0){
            act_data.data.x = (signed int)buf_data.data.x * -1;
        }else{
            act_data.data.x = buf_data.data.x;
        }
        
        if(buf_data.data.vy == 0){
            act_data.data.y = (signed int)buf_data.data.y * -1;
        }else{
            act_data.data.y = buf_data.data.y;
        }
        
        if(buf_data.data.vt == 0){
            act_data.data.t = (signed int)buf_data.data.t * -1;
        }else{
            act_data.data.t = buf_data.data.t;
        }
        act_data.data.au = buf_data.data.au;
        act_data.data.ad = buf_data.data.ad;
        act_data.data.h = buf_data.data.h;
        act_data.data.set = buf_data.data.set;
        act_data.data.s = buf_data.data.s;
    }
    
            
            
}


void drive_mech(){
    float power,lduty,turn;
    power = act_data.data.y * (PWMMAX / daMAX);
    lduty = act_data.data.x * (PWMMAX / daMAX);
    turn = act_data.data.t * (PWMMAX / daMAX);
    if(power == 0.00){
        if(turn < 0){
            dirR = 1;
            dirL = 1;
            pwrR = turn * -1;
            pwrL = turn * -1;
        }else if(turn >= 0){
            dirR = 0;
            dirL = 0;
            pwrR = turn;
            pwrL = turn;    
        }
    }else{ 
        if(power <= 0){
            power = power * -1;
            dirR = 1;
            dirL = 0;
        }else if (power > 0){
            dirR = 0;
            dirL = 1;
        }
    
        if(lduty <= 0){
            pwrR = power;
            pwrL = power - (lduty * -1) / 2;   
        }else if(lduty > 0){
            pwrR = power - lduty / 2; 
            pwrL = power;
        }    
        //comUSB.printf("p %f, l %f,t %f\r\n",power,lduty,turn);
    }
}


void seiyaho(){
    if(act_data.data.au == 1){
        if(angMAX != 1){
            dirA = 1;
            pwrA = angpwr;
        }else if(angMAX == 1){
            pwrA = 0.00;
        }
    }else if(act_data.data.ad == 1){
        if(angMIN != 1){
            dirA = 0;
            pwrA = angpwr;
        }else if(angMIN == 1){
            pwrA = 0.00;
        }
    }else if(act_data.data.au != 1 && act_data.data.ad != 1) {
        pwrA = 0.00;
    }
            
}


void hold(){
    if(act_data.data.h == 1 && act_data.data.s != 1 && act_data.data.s != 1){
        servo.pulsewidth_us(SERVO_MIN_ANG + 900);
    }else if(act_data.data.h != 1 && act_data.data.set != 1){
        servo.pulsewidth_us(SERVO_MIN_ANG + 500 * (0 / 90));
    }else if(act_data.data.set == 1 && act_data.data.s != 1 && act_data.data.s != 1){
        servo.pulsewidth_us(SERVO_MIN_ANG + 1280);
    }
}


void fire(){
    if(act_data.data.s == 1 && act_data.data.h != 1 && act_data.data.set != 1){
        loader = 0;
        shoter = 1;
    }else if(act_data.data.s == 0 && act_data.data.h != 1 && act_data.data.set != 1){
        loader = 1;
        shoter = 0;
    }
}


int main() {
        Serialsetup();
        servo.period_ms(20);
    while(1) {
        bufcheck = 0;
        com1.attach(interface,Serial::RxIrq);
        fire();
        drive_mech();
        seiyaho();
        hold();
        /*comUSB.printf("x %d,y %d,t %d,u %d,d %d,h %d,%d,s ,%d %d,%d,%d\r\n",
                        act_data.data.x,act_data.data.y,act_data.data.t,
                        act_data.data.au,act_data.data.ad,act_data.data.h,act_data.data.set,
                        act_data.data.s,buf_data.data.vx,buf_data.data.vy,buf_data.data.vt);*/
        
    }
}