File content as of revision 0:d527838453f6:

#include "mbed.h"
#include "QEI.h"
#include "i2cslave.h"
#include "PID.h"
#include "define.h"

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

union  UNION
{
    int integer;
    char c[6];
};

UNION PM_target;
UNION RE_target;
UNION gain;
UNION PM_ofset;

char Registar[128] = {};
QEI rorikon(dp25, dp17, NC, 50, QEI::X4_ENCODING);
//AnalogIn   potentiometer(dp10);
AnalogIn   potentiometer(dp11);
i2cslave myi2c(dp5, dp27, Registar);
int getmode_PM_angle(const int kaisu);
int checker = 0;
void check();

int main()
{
    NVIC_SetPriority(EINT3_IRQn, 30);
    NVIC_SetPriority(EINT2_IRQn, 30);
    NVIC_SetPriority(EINT1_IRQn, 30);
    NVIC_SetPriority(EINT0_IRQn, 30);
    NVIC_SetPriority(TIMER_16_0_IRQn, 20);
    NVIC_SetPriority(TIMER_16_1_IRQn, 20);
    NVIC_SetPriority(TIMER_32_0_IRQn, 20);
    NVIC_SetPriority(TIMER_32_1_IRQn, 20);
    NVIC_SetPriority(I2C_IRQn, 10);
    NVIC_SetPriority(UART_IRQn, 5);
    
    /*BusOut PM_motor(dp4, dp9, dp13, dp14);
    BusOut RE_motor(dp25, dp24, dp18, dp17);*/
    BusOut PM_motor(dp14, dp13, dp10, dp9);
    BusOut RE_motor(dp28, dp26, dp24, dp18);
    PwmOut PM_pwm(dp2);
    PwmOut RE_pwm(dp1);
    
    Ticker tic;
    tic.attach(check, 0.1);
    Timer t;
    PID PM_PID(&t);
    PID RE_PID(&t);
    PM_PID.set_parameter(0.0200, 0.43);//0.0200, 0.43//0.0200, 0.4   //0.0400, 0.317460317
    //RE_PID.set_parameter(0.001, 0.07);
    //RE_PID.set_parameter(0.00070, 0.30);//0.00074, 0.35 //0.00074, 0.277777778  //parameter of 1500
    //RE_PID.set_parameter(0.00025, 0.37); //0.00040, 0.344827586   //parameter of 300
    float pm_pid = 0;
    float re_pid = 0;
    float RE_angle = 0;
    float prev_RE_angle = 0;
    int PM_angle = 0;
    float nowtime = 0;
    float prev_time = 0;
    float RE_speed = 0;
    int target = 1800;
    int prev_target = 1800;
    float Threshold = 5.5;
    float RE_Threshold = 5;
    /*****************************************/
    myi2c.address(MD1_addr);
    /*****************************************/
    myi2c.frequency(1000000);
    
    PM_ofset.integer = getmode_PM_angle(128);
    for(int i = 0; i < 4; i++)
    {
        Registar[PM_ofset_reg + i] = PM_ofset.c[i];
    }
    printf("start");
    t.start();
    while(1)
    {
        PM_angle = getmode_PM_angle(50);
        for(int i = 0; i < 5; i++)
        {
            PM_target.c[i] = Registar[PM_target_reg + i];
            RE_target.c[i] = Registar[RE_target_reg + i];
            gain.c[i] = Registar[Kp + i];
        }
        prev_target = target;
        target = PM_target.integer;
        /*if(!Registar[Joystick_ctrl] &&(abs((target - PM_ofset.integer) % 45) != 0))
        {
            target = prev_target;
        }*/
        //printf("%d.%d%d%d", PM_angle, target, (int)(PM_PID.Ki*10000), (int)nowtime);
        prev_RE_angle = RE_angle;
        prev_time = nowtime;
        RE_angle = rorikon.getSumangle();
        nowtime = t.read();
        RE_speed = (RE_angle - prev_RE_angle)/(nowtime - prev_time);
        printf("%f%d%d", RE_speed, gain.integer, (int)nowtime);
        switch(RE_target.integer)
        {
            case 1500:
                RE_PID.set_parameter(0.00070, 0.30);
                break;
            case 300:
                RE_PID.set_parameter(0.00025, 0.37);
                break;
        }
        printf("%d", Registar[wheel_start]);
        if(Registar[wheel_start] == 2)
        {
            re_pid = RE_PID.PI_lateD((-1 * (float)RE_target.integer), RE_speed);//, ((float)gain.integer)/10000);
        }
        else if(Registar[wheel_start] == 1)
        {
            re_pid = RE_PID.PI_lateD((float)RE_target.integer, RE_speed);//, ((float)gain.integer)/10000);
        }
        /*else if(Registar[pid_start] && ( (RE_speed < (RE_target.integer - RE_Threshold)) || (RE_speed > (RE_target.integer + RE_Threshold)) ) )
        {
            re_pid = RE_PID.control_P((float)RE_target.integer, RE_speed, (float)gain.integer/100000);
        }*/
        else
        {
            re_pid = 0;
            RE_PID.reset();
            //pid = mypid.control_P(0, PM_angle, 0.005);
        }
        
        if(re_pid > 0.00005)
        {
            RE_motor = 0;
            wait_ms(5);
            RE_pwm = re_pid;
            RE_motor = 10;
        }
        else if(re_pid < -0.00005)
        {
            RE_motor = 0;
            wait_ms(5);
            RE_pwm = re_pid * (-1);
            RE_motor = 5;
        }
        else
        {
            RE_motor = 0;
            wait_ms(5);
            RE_pwm = 1;
            RE_motor = 3;
        }
        
        
        if( ( PM_angle > (target - Threshold) ) && ( PM_angle < (target + Threshold) ) )
        {
            pm_pid = 0;
            PM_PID.reset();
            PM_motor = 0;
            wait_ms(1);
            PM_motor = 3;
            PM_pwm = 1;
        }
        
        if(Registar[PM_start])
        {
            pm_pid = PM_PID.control_P(target, PM_angle, 0.007);
        }
        if(Registar[PM_start] && Registar[pid_start] && ( (PM_angle < (target - Threshold)) || (PM_angle > (target + Threshold)) ) )
        {
            pm_pid = PM_PID.PI_lateD(target, PM_angle);//, ((float)gain.integer)/10000);
        }
        else if(Registar[pid_start])
        {
            pm_pid = 0;
            PM_PID.reset();
            //pid = mypid.control_P(0, PM_angle, 0.005);
        }
        
        if(pm_pid > 0.04)
        {
            PM_motor = 0;
            wait_ms(1);
            PM_pwm = pm_pid;
            PM_motor = 5;
        }
        else if(pm_pid < -0.04)
        {
            PM_motor = 0;
            wait_ms(1);
            PM_pwm = pm_pid * (-1);
            PM_motor = 10;
        }
        else
        {
            PM_motor = 0;
            wait_ms(1);
            PM_pwm = 1;
            PM_motor = 3;
            PM_PID.reset();
        }
        printf("\r\n");
        wait_ms(60);
    }
}

int getmode_PM_angle(const int kaisu)
{
    int PMangle[128] = {};
    int samecount[128] = {};
    int tmpcount[128] = {};
    int num = 0;
    for(int i = 0; i < kaisu; i++)
    {
        PMangle[i] = (int)(potentiometer * 3600 + 0.5);
    }
    for(int i = 0; i < kaisu; i++)
    {
        for(int j = 0; j < kaisu; j++)
        {
            if(PMangle[i] == PMangle[j])
            {
                samecount[i]++;
            }
        }
        tmpcount[i] = samecount[i];
    }
    for(int i = 0; i < kaisu; i++)
    {
        for(int j = 0; j < i; j++)
        {
            if(tmpcount[i] > tmpcount[j])
            {
                int tmp = tmpcount[j];
                tmpcount[j] = tmpcount[i];
                tmpcount[i] = tmp;
            }
        }
    }
    int mostcount = tmpcount[0];
    for(int i = 0; i < kaisu; i++)
    {
        if(samecount[i] == mostcount)
        {
            num = i;
        }
    }
    return PMangle[num];
}

void check()
{
    Registar[check_reg]++;
    if(Registar[check_reg] > 2)
    {
        NVIC_SystemReset();
    }
}