File content as of revision 1:755d8977d4a3:

#include "mbed.h"
#include "PS3.h"
#include "i2cmaster.h"
#include "define.h"

#define Enter 13
#define BackSpace 8
#define Space 32

i2c myi2c(p28, p27);

int i2ccheck[4] = {};
int timer = 0;

void check()
{
    timer++;
    /*Slave puted data first by mother cause error*/
    myi2c.WriteSomeData(0xFE, check_reg, 0, 1);     //0xFE is imaginary　address
    /*then, I puted imaginary address yhe!*/
    myi2c.WriteSomeData(MD1_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD2_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD3_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD4_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD5_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD6_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD7_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD8_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD9_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(MD10_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(SE1_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(SE2_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(CY1_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(SERVO1_addr, check_reg, 0, 1);
    myi2c.WriteSomeData(0xFE, check_reg, 0, 1);
}

union UNION
{
    int integer;
    char character[4];
};
UNION PM_target[4];
UNION PM_ofset[4];
UNION RE_target[4];
UNION gain;

Serial pc(USBTX, USBRX); 
int pcdata = 0;
int prev_pcdata = 0;
void getpc();

DigitalOut sbdbt_battery(p21);
DigitalOut xbee_battery(p5);

int main()
{
    BusOut leds(LED1, LED2, LED3, LED4);
    NVIC_SetPriority(TIMER0_IRQn, 20);
    NVIC_SetPriority(TIMER1_IRQn, 20);
    NVIC_SetPriority(TIMER2_IRQn, 20);
    NVIC_SetPriority(TIMER3_IRQn, 20);
    NVIC_SetPriority(UART0_IRQn, 10);
    NVIC_SetPriority(UART1_IRQn, 10);
    NVIC_SetPriority(UART2_IRQn, 10);
    NVIC_SetPriority(UART3_IRQn, 10);
    PS3 sbdbt(p13, p14);
    PS3 xbee(p9, p10);
    sbdbt_battery   = 1;
    sbdbt.baud(9600);
    xbee_battery    = 1;
    xbee.baud(9600);
    //xbee.addattach(sbdbt_to_xbee);
    //sbdbt.addattach(xbee_to_sbdbt);
    pc.attach(getpc);
    myi2c.frequency(1000000);
    Ticker t;
    t.attach(check, 0.1);
    int prev_PM_target[4] = {};
    char sensor1 = 0;
    char sensors[8] = {};
    char sensorsflag[8] = {};
    char sensor2 = 0;
    int gainup = 0;
    int Dpad_deg = 0;
    bool Dpadctrl_flag = 1;
                             //0,  ue, sita
    int Dpad_table[3][3] = {{  0,   0,   180}, //0
                            { 90,  45,  135}, //migi
                            {-90, -45, -135}};//hidari
    int wheel[4] = {};
    int torikomi_taosu_motor = 0;
    int modechange = 0;
    int modechangeflag = 1;
    bool counterwheel = 0;
    bool l1 = 0;
    bool r1 = 0;
    bool l2 = 0;
    bool r2 = 0;
    bool Maru = 0;
    bool Batu = 0;
    bool Sankaku = 0;
    int sankakuflag = 0;
    bool Sikaku = 0;
    bool Ue = 0;
    bool Sita = 0;
    bool Migi = 0;
    bool Hidari = 0;
    int RightY = 0;
    int LeftY = 0;
    int LeftX = 0;
    wait_ms(500);
    
    for(int address = 0; address < 4; address++)
    {
        while(PM_ofset[address].integer < 1750 || PM_ofset[address].integer > 1850)
        {
            for(int i = 0; i < 4; i++)
            {
                myi2c.getSlaveRegistarData(MD1_addr + (0x02*address), PM_ofset_reg + i, &PM_ofset[address].character[i], 1);
                wait_ms(20);
            }
        }
        PM_target[address].integer = PM_ofset[address].integer;
    }
    while(1)
    {
        leds = modechange;
        if( modechangeflag && ( sbdbt.getSELECTState() | xbee.getSELECTState() ) )
        {
            modechange++;
            if(modechange == 4)modechange = 0;
            modechangeflag = 0;
            wait_ms(500);
        }
        modechangeflag = !( sbdbt.getSELECTState() | xbee.getSELECTState() );
        Maru = sbdbt.getButtonState(maru) | xbee.getButtonState(maru);
        Batu = sbdbt.getButtonState(batu) | xbee.getButtonState(batu);
        Sankaku = (sbdbt.getButtonState(sankaku) | xbee.getButtonState(sankaku)) & modechangeflag;
        Sikaku = (sbdbt.getButtonState(sikaku) | xbee.getButtonState(sikaku)) & modechangeflag;
        l1 = sbdbt.getButtonState(L1) | xbee.getButtonState(L1);
        r1 = sbdbt.getButtonState(R1) | xbee.getButtonState(R1);
        l2 = sbdbt.getButtonState(L2) | xbee.getButtonState(L2);
        r2 = sbdbt.getButtonState(R2) | xbee.getButtonState(R2);
        Ue = sbdbt.getButtonState(ue) | xbee.getButtonState(ue);
        Sita = sbdbt.getButtonState(sita) | xbee.getButtonState(sita);
        Migi = sbdbt.getButtonState(migi) | xbee.getButtonState(migi);
        Hidari = sbdbt.getButtonState(hidari) | xbee.getButtonState(hidari);
        RightY = sbdbt.getRightJoystickYaxis();
        LeftY = sbdbt.getLeftJoystickYaxis();
        LeftX = sbdbt.getLeftJoystickXaxis();
        for(int i = 0; i < 8; i++)
            sensors[i] = (sensor1>>i) & 1;
            
        switch(modechange)
        {
            case 0: //start to area1 clear
                /*LeftY :  tukamuyatu spin**********************************/
                if(LeftY > 0)
                {
                    myi2c.WriteSomeData(MD5_addr, motor1_state, 5, 1);
                }
                else if(LeftY < 0)
                {
                    myi2c.WriteSomeData(MD5_addr, motor1_state, 10, 1);
                    LeftY = LeftY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD5_addr, motor1_state, 3, 1);
                }
                myi2c.WriteSomeData(MD5_addr, motor1_pwm, LeftY * 4-1, 1);
                
                /*RightY : rail slide***********************************/
                if(RightY > 0)
                {
                    myi2c.WriteSomeData(MD5_addr, motor2_state, 5, 1);
                }
                else if(RightY < 0)
                {
                    myi2c.WriteSomeData(MD5_addr, motor2_state, 10, 1);
                    RightY = RightY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD5_addr, motor2_state, 3, 1);
                }
                myi2c.WriteSomeData(MD5_addr, motor2_pwm, RightY * 4-1, 1);
                
                /*Sikaku : cylinder*****************************************/
                myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
                break;
            case 1: //box torikomi to bridge
                /*Sankaku : torikomi taosu********************************************************/
                myi2c.WriteSomeData(MD6_addr, motor2_pwm, 255, 1);
                if(Sankaku == 0)
                {
                    sankakuflag = 1;
                }
                if(sankakuflag && Sankaku)
                {
                    torikomi_taosu_motor++;
                    if((torikomi_taosu_motor % 2))
                    {
                        myi2c.WriteSomeData(MD6_addr, motor2_state, 5, 1);  //torikomi taosu
                    }
                    else
                    {
                        myi2c.WriteSomeData(MD6_addr, motor2_state, 10, 1);  //torikomi modosu
                    
                    }
                    sankakuflag = 0;
                }
                if(sensors[0] == 0)
                {
                    sensorsflag[0] = 1;
                }
                if((sensorsflag[0] == 1 && sensors[0] == 1) || !Sankaku)
                {
                    myi2c.WriteSomeData(MD6_addr, motor2_state, 3, 1);  //torikomi taosi stop
                    sensorsflag[0] = 0;
                }
                
                /*LeftY : torikomi slide**********************************/
                if(LeftY > 0)
                {
                    myi2c.WriteSomeData(MD6_addr, motor1_state, 5, 1);
                }
                else if(LeftY < 0)
                {
                    myi2c.WriteSomeData(MD6_addr, motor1_state, 10, 1);
                    LeftY = LeftY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD6_addr, motor1_state, 3, 1);
                }
                myi2c.WriteSomeData(MD6_addr, motor1_pwm, LeftY * 4-1, 1);
                
                /*Sikaku : cylinder*********************************************/
                myi2c.WriteSomeData(CY1_addr, motor1_state, Sikaku, 1);
                break;
            case 2: //bridge to area3
                myi2c.WriteSomeData(SERVO1_addr, motor1_state, (!Sankaku) | (Sankaku << 1) | ((RightY > 60) << 2) | (Sikaku << 3), 1);
                /*RightY : bridge1***********************************************
                if(RightY > 0)
                {
                    myi2c.WriteSomeData(MD7_addr, motor1_state, 5, 1);
                }
                else if(RightY < 0)
                {
                    myi2c.WriteSomeData(MD7_addr, motor1_state, 10, 1);
                    RightY = RightY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD7_addr, motor1_state, 3, 1);
                }
                myi2c.WriteSomeData(MD7_addr, motor1_pwm, RightY * 4-1, 1);
                
                /*LeftY : bridge2******************************************************
                if(LeftY > 0)
                {
                    myi2c.WriteSomeData(MD7_addr, motor2_state, 5, 1);
                }
                else if(LeftY < 0)
                {
                    myi2c.WriteSomeData(MD7_addr, motor2_state, 10, 1);
                    LeftY = LeftY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD7_addr, motor2_state, 3, 1);
                }
                myi2c.WriteSomeData(MD7_addr, motor2_pwm, LeftY * 4-1, 1);*/
                break;
            case 3: //box pile up
                /*LeftX : jack move*************************************************/
                if(!Sankaku)
                {
                    if(LeftX > 0)
                    {
                        myi2c.WriteSomeData(MD8_addr, motor1_state, 5, 1);
                    }
                    else if(LeftX < 0)
                    {
                        myi2c.WriteSomeData(MD8_addr, motor1_state, 10, 1);
                        LeftX = LeftX * (-1);
                    }
                    else
                    {
                        myi2c.WriteSomeData(MD8_addr, motor1_state, 3, 1);
                    }
                    myi2c.WriteSomeData(MD8_addr, motor1_pwm, LeftX * 4-1, 1);
                }
                
                /*Sikaku : jack up***************************************************/
                myi2c.WriteSomeData(MD8_addr, motor2_state, Sikaku * 5, 1);
                myi2c.WriteSomeData(MD8_addr, motor2_pwm, 255, 1);
                
                /*RightY : rail up***************************************************/
                if(RightY > 0)
                {
                    myi2c.WriteSomeData(MD9_addr, motor1_state, 5, 1);
                    myi2c.WriteSomeData(MD9_addr, motor2_state, 5, 1);
                }
                else if(RightY < 0)
                {
                    myi2c.WriteSomeData(MD9_addr, motor1_state, 10, 1);
                    myi2c.WriteSomeData(MD9_addr, motor2_state, 10, 1);
                    RightY = RightY * (-1);
                }
                else
                {
                    myi2c.WriteSomeData(MD9_addr, motor1_state, 3, 1);
                    myi2c.WriteSomeData(MD9_addr, motor2_state, 3, 1);
                }
                myi2c.WriteSomeData(MD9_addr, motor1_pwm, RightY * 4-1, 1);
                myi2c.WriteSomeData(MD9_addr, motor2_pwm, RightY * 4-1, 1);
                
                /*Sankaku & LeftY : center slide**************************************/
                if(Sankaku)
                {
                    if(LeftY > 0)
                    {
                        myi2c.WriteSomeData(MD10_addr, motor1_state, 5, 1);
                    }
                    else if(LeftY < 0)
                    {
                        myi2c.WriteSomeData(MD10_addr, motor1_state, 10, 1);
                        LeftY = LeftY * (-1);
                    }
                    else
                    {
                        myi2c.WriteSomeData(MD10_addr, motor1_state, 3, 1);
                    }
                    myi2c.WriteSomeData(MD10_addr, motor1_pwm, LeftY * 4-1, 1);
                }
                break;
        }
        
        /*Steering******************************************************/
        Dpad_deg = Dpad_table[Migi + 2*Hidari][Ue + 2*Sita];
        
        if(Dpad_deg < -90)
        {
            Dpad_deg += 180;
            counterwheel = 1;
        }
        else if(Dpad_deg > 90)
        {
            Dpad_deg -= 180;
            counterwheel = 1;
        }
        else
        {
            counterwheel = 0;
        }
        
        
        if(Ue | Sita | Migi | Hidari)
        {
            Dpadctrl_flag = 1;
        }
        else
        {
            Dpadctrl_flag = 0;
        }
        if(Batu)
        {
            for(int i = 2; i < 4; i++)
            {
                prev_PM_target[i] = PM_target[i].integer;
                if(Dpadctrl_flag)
                {
                    //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
                    PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
                }
                RE_target[i].integer = 300 * Maru;
            }   
        }
        else
        {
            for(int i = 0; i < 4; i++)
            {
                prev_PM_target[i] = PM_target[i].integer;
                if(Dpadctrl_flag)
                {
                    //PM_target[i].integer = (int)((Dpad_deg > 0)? Dpad_deg + 0.5 : Dpad_deg - 0.5) + PM_ofset[i].integer;
                    PM_target[i].integer = Dpad_deg + PM_ofset[i].integer;
                }
                RE_target[i].integer = 300 * Maru;
            }
        }
        if(Maru)
        {
            wheel[0] = 2 - counterwheel;
            wheel[1] = 1 + counterwheel;
            wheel[2] = 1 +  counterwheel;
            wheel[3] = 2 - counterwheel;
            
            if(l2)
            {
                wheel[0] = 1;
                wheel[1] = 1;
                wheel[2] = 1;
                wheel[3] = 1;
            }
            if(r2)
            {
                wheel[0] = 2;
                wheel[1] = 2;
                wheel[2] = 2;
                wheel[3] = 2;
            }
        }
        /*else if(Batu)
        {
            wheel[0] = 1 + counterwheel;
            wheel[1] = 2 - counterwheel;
            wheel[2] = 2 - counterwheel;
            wheel[3] = 1 + counterwheel;
        }*/
        else
        {
            wheel[0] = 0;
            wheel[1] = 0;
            wheel[2] = 0;
            wheel[3] = 0;
        }
        if(r1)
        {
            if(Batu)
            {
                for(int i = 2; i < 4; i++)
                PM_target[i].integer += 5;
                Dpadctrl_flag = 0;
            }
            else
            {
                for(int i = 0; i < 4; i++)
                PM_target[i].integer += 5;
                Dpadctrl_flag = 0;
            }
        }
        else if(l1)
        {
            if(Batu)
            {
                for(int i = 2; i < 4; i++)
                PM_target[i].integer -= 5;
                Dpadctrl_flag = 0;
            }
            else
            {
                for(int i = 0; i < 4; i++)
                PM_target[i].integer -= 5;
                Dpadctrl_flag = 0;
            }
        }
        else if(r2 | l2)
        {
            PM_target[0].integer = 45 + PM_ofset[0].integer;
            PM_target[1].integer = -45 + PM_ofset[1].integer;
            PM_target[2].integer = 45 + PM_ofset[2].integer;
            PM_target[3].integer = -45 + PM_ofset[3].integer;
            Dpadctrl_flag = 0;         
        }                
        /*wheel communication*************************************/
        
        for(int address = 0; address < 4; address++)
        {
            myi2c.WriteSomeData(MD1_addr + (0x02*address), wheel_start, wheel[address], 1);
            if(PM_target[address].integer != prev_PM_target[address])
            {
                myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 0, 1);
            }
            
            for(int i = 0; i < 4; i++)
            {
                myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_target_reg + i, PM_target[address].character[i], 1);
                myi2c.WriteSomeData(MD1_addr + (0x02*address), RE_target_reg + i, RE_target[address].character[i], 1);
            }
            //myi2c.WriteSomeData(MD1_addr + (0x02*address), pid_start, 1, 1);
            myi2c.WriteSomeData(MD1_addr + (0x02*address), PM_start, 1, 1);
        }
        myi2c.getSlaveRegistarData(SE1_addr, 0, &sensor1, 1);
        wait_ms(20);
        myi2c.getSlaveRegistarData(SE2_addr, 0, &sensor2, 1);
        wait_ms(20);
    }
}

void getpc()
{
    if(pc.readable())
    {
        prev_pcdata = pcdata;
        pcdata = pc.getc();
    }
}