/*This file has been prepared for Doxygen automatic documentation generation.*/

#include "header/includes.h"
#include "altera_avalon_uart.h"

alt_u8 led = 0x2;
alt_u8 dir = 0;

void led_flash(void)
{
    if (led & 0x81) 
    {
      dir = (dir ^ 0x1);
    }

    if (dir)
    {
      led = led >> 1;
    }
    else
    {
      led = led << 1;
    }
    IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE, led);
}
/********************************************************************
                         PID計(jì)算相關(guān)參數(shù)定義
*********************************************************************/
#define PID_PERIOD 8000
//#define PID_PERIOD 8000
#define HOERF 0
#define HOERM 1
#define HOERL 2

volatile long counter=0;   //編碼器的計(jì)數(shù)器
uchar go_en=0;    //使能走
//int clk_cnt=0;   //輸出脈沖計(jì)數(shù)器
volatile long locate = 0; //位置信息，調(diào)節(jié)用
volatile long Initposition = 0;  //初始化后的位置

volatile int  vd = -30,vq = 30;
volatile int Speed=0;    //速度參數(shù)
struct PID_DATA piddata;

alt_16 timeren;
alt_16 timercnt;
alt_16 timeren10;


/********************************************************************
                            電機(jī)控制和定時(shí)中斷函數(shù)
*********************************************************************/
long get_addr(void)
{
    int addr,circle;
    addr = IORD_ALTERA_AVALON_PIO_DATA(ABADDR_BASE);
    circle = IORD_ALTERA_AVALON_PIO_DATA(CIRCLES_BASE);
    counter = (circle - 0x800)*1500L + addr;
    return counter;
}

void Mspeed(int spd) //調(diào)速
{
    //為了不超調(diào)，將vd和vq的值都限制在-600~600之間。
    //spd = -spd;
    if(spd >= 0)
    {
        if(spd > 600)
            spd = 600;
        vd = -spd*3/2;
        vq = spd;
    }
    else
    {
        if(spd < -600)
            spd = -600;
        vd = -spd*6/5;
        vq = spd;
    }

    IOWR_ALTERA_AVALON_PIO_DATA(VD_BASE, vd);
    IOWR_ALTERA_AVALON_PIO_DATA(VQ_BASE, vq);
}

void timer_0_IRQ(void *context, alt_u32 id)
{
    //alt_putstr("Hello from Nios II!\n");
    
    static long e[2];
    long deta;
    char limit_tmp;
    //IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE, 1);
    IOWR_ALTERA_AVALON_TIMER_STATUS( TIMER_0_BASE,0 );
    timeren = 1;
    timercnt++;

    //get_addr();
    deta = get_addr() - locate;
    /*
    //if(temp >= -1 && temp <= 1) pid_Reset_Integrator(&piddata);
    //if(temp == 0) pid_Reset_Integrator(&piddata);
    //disp(temp,8);
    e[1] = deta - e[0];
    e[0] = deta;
    fuzzy_step(e);
    pid_factor(crisp_scale[0],crisp_scale[1], crisp_scale[2], &piddata);
    */
    //Speed = pid_Controller(locate,counter,&piddata);
    //pid_factor(100, 50, 10, &piddata);
    
    //deta = get_addr() - locate;

    Speed = pid_Controller(locate,counter,&piddata);
    
    if(deta >= -1 && deta <= 1)
    //if(deta == 0)
    {
        //if(deta == 0)
            pid_Reset_Integrator(&piddata); 
        Speed = 0;
    }
    //else
    {
        
    }
    
    limit_tmp = IORD_ALTERA_AVALON_PIO_DATA(LIMIT_BASE);
    if(limit_tmp & (1<<HOERL))  //最后的霍爾元件報(bào)警，速度只能為負(fù)
    {
        if(Speed > 0)
        {
            Mspeed(0);
            Speed = 0;
        }
        else
            Mspeed(Speed);
    }
    else if(limit_tmp & ((1<<HOERM)|(1<<HOERF)))
    {
        if(Speed < 0)
        {
            Mspeed(0);
            Speed = 0;
        }
        else
            Mspeed(Speed);
    }
    else
        Mspeed(Speed);
    
    if(timercnt == 100000000/PID_PERIOD)
    {
        timercnt = 0;
        timeren10 = 1;
    }
    //IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE, 0);
}
/********************************************************************
                               步進(jìn)電機(jī)控制函數(shù)
*********************************************************************/
alt_16 Stlocation;
void Stepper(void)
{
    char i;

    alt_16 st_limit, st_position;
    st_limit = IORD_ALTERA_AVALON_PIO_DATA(STLIMIT_BASE);
    if(st_limit)
    {
        st_position = IORD_ALTERA_AVALON_PIO_DATA(STPOSITION_BASE);
        if(st_limit & 0x01)  //底下開關(guān)關(guān)閉，限制往下走
        {
            if(Stlocation > st_position)
            {
                IOWR_ALTERA_AVALON_PIO_DATA(STSTOP_BASE, 1);
                IOWR_ALTERA_AVALON_PIO_DATA(STLOCATE_BASE, Stlocation);
                IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                for(i=0;i<4;i++)
                {
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
                }
            }
        }
        else if(st_limit & 0x02)//頂部開關(guān)關(guān)閉，限制往上走
        {
            if(Stlocation < st_position)
            {
                IOWR_ALTERA_AVALON_PIO_DATA(STSTOP_BASE, 1);
                IOWR_ALTERA_AVALON_PIO_DATA(STLOCATE_BASE, Stlocation);
                IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                for(i=0;i<4;i++)
                {
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
                    IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
                }
            }
        }
    }
    else
    {
        IOWR_ALTERA_AVALON_PIO_DATA(STSTOP_BASE, 1);
        IOWR_ALTERA_AVALON_PIO_DATA(STLOCATE_BASE, Stlocation);
        IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
        for(i=0;i<4;i++)
        {
            IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
            IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 0);
            IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
            IOWR_ALTERA_AVALON_PIO_DATA(STLE_BASE, 1);
        }
    }
}

void Stepper_init(void)
{
    char i;
    IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 0);
    for(i=0;i<100;i++);
    IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 1);
    IOWR_ALTERA_AVALON_PIO_DATA(STSTOP_BASE, 1);
}
/********************************************************************
                             主機(jī)串口處理函數(shù)
*********************************************************************/
alt_16 tmp=0,rcvcnt=0,chk0 = 0;
alt_16 state0 = 0;
alt_16 xaa,x55;  //識(shí)別0x55和0xaa用
alt_16 update;
char rcv[10];
alt_16 rcvlength;
alt_16 Sttodo;  //步進(jìn)電機(jī)位置更新未執(zhí)行為1

void do_receive(void)
{
    alt_16 i;
    alt_32 rcvtmp;
    rcvtmp = 0;
    for(i=0;i<4;i++)
    {
        rcvtmp <<= 8;
        rcvtmp |= rcv[i];
    }
    locate = rcvtmp + Initposition;  //伺服電機(jī)位置更新
    rcvtmp = 0;
    
    for(i=0;i<2;i++)
    {
        rcvtmp <<= 8;
        rcvtmp |= rcv[i+4];
    }
    Stlocation = rcvtmp;  //步進(jìn)電機(jī)位置更新
    Stepper();
}

void usart0_do(void)
{
    if(rx_counter0)
    {
        tmp = get_char0();
        if(state0 == 0)
        {
            if(tmp == 0x55) //接收到數(shù)據(jù)包起始標(biāo)志位
            {
                state0 = 1;
                rcvcnt = 0;  //準(zhǔn)備接收數(shù)據(jù)
                rcvlength = 0;
                xaa = 0;
                x55 = 0;
                chk0 = 0;
            }
        }
        else if(state0 == 1)
        {
            rcvlength = tmp;  //以第一個(gè)數(shù)據(jù)為有效數(shù)據(jù)長度，有校驗(yàn)
            chk0 += tmp;
            state0 = 2;
        }
        else if(state0 == 2) //接收數(shù)據(jù)狀態(tài)
        {
            if(tmp != 0xaa)
            {
                if(xaa == 0xaa)  //說明前一個(gè)數(shù)是0xaa
                {
                    rcv[rcvcnt] = x55;
                    rcvcnt++; //接收計(jì)數(shù)器
                    xaa = 0;
                }
                rcv[rcvcnt] = tmp;
                chk0 += tmp; //校驗(yàn)
                rcvcnt++; //接收計(jì)數(shù)器
                xaa = 0;
                if(rcvcnt >= rcvlength)
                    state0 = 4;
                if(tmp == 0x55)  //出錯(cuò)
                    state0 = 0;
            }
            else
            {
                if(xaa == 0xaa)  //說明接收到連續(xù)兩個(gè)0xaa，否則aa為0
                {
                    x55 = 0xaa;
                    chk0 += 0xaa;  //校驗(yàn)
                }
                else  //第一次接到0xaa
                {
                    x55 = 0x55;
                    chk0 += 0xaa;  //校驗(yàn)
                    xaa = 0xaa;
                }
            }
        }
        else if(state0 == 3) //暫時(shí)沒有校驗(yàn)
        {
            if(chk0 == tmp)
                state0 = 4;
            else
                state0 = 0;
        }
        else if(state0 == 4)
        {
            if(tmp == 0x55)
            {
                state0 = 0;   //結(jié)束，回到起始狀態(tài)
                update = 1;   //完成一次接收，準(zhǔn)備數(shù)據(jù)更新
                chk0 = 0;
                put_char0(0x55);
            }
        }
        else
        {
            state0 = 0;
        }
    }
    //數(shù)據(jù)更新
    if(update)
    {
        update = 0;
        do_receive();
    }
}
/********************************************************************
                               初始化
*********************************************************************/
void timer_init(void)  //100Hz
{
    alt_irq_register(TIMER_0_IRQ,0,timer_0_IRQ);
    //初始化周期寄存器
    IOWR_ALTERA_AVALON_TIMER_PERIODL(TIMER_0_BASE,PID_PERIOD&0x0000FFFF);
    IOWR_ALTERA_AVALON_TIMER_PERIODH(TIMER_0_BASE,(PID_PERIOD>>16)&0x0000FFFF);
    //打開中斷、循環(huán)計(jì)數(shù)、定時(shí)開始
    IOWR_ALTERA_AVALON_TIMER_CONTROL(TIMER_0_BASE,(ALTERA_AVALON_TIMER_CONTROL_ITO_MSK|ALTERA_AVALON_TIMER_CONTROL_START_MSK|ALTERA_AVALON_TIMER_CONTROL_CONT_MSK));
}



/********************************************************************
                           顯示讀取的sin值
*********************************************************************/
void disp_long(long temp)
{
    alt_16 bit[7];
    alt_16 i = 0;
    for(;i<6;i++)
    {
        bit[5-i] = temp%10;
        temp /= 10;
    }
    for(i=0;i<6;i++)
    {
        put_char0(bit[i]+'0');
    }
}
void disp(void)
{
    put_str0("\nlocate = ");
    disp_long(locate);
    put_str0("; counter = ");
    disp_long(counter);
    put_str0("; speed = ");
    disp_long(Speed);
    put_str0("; Initposition = ");
    disp_long(Initposition);
}

/********************************************************************
                               主函數(shù)
*********************************************************************/
char Init_PMSM_succeed = 0;
char Init_Stepper_succeed = 0;
void Init(void)
{
    pid_Init(100, 100, 50, &piddata);
    timer_init();
    uart0_init();
    Stepper_init();
    locate = 0;

    Stlocation = -0x8000;
    Stepper();
}

int main(void)
{
    char i;
    alt_16 Itimer = 0;
    alt_16 stlimit0 = 0, stlimit1 = 0; 
    alt_putstr("Hello from Nios II!\n");
    Init();
    while(1)
    {
        //led_flash();
        if(Init_PMSM_succeed && Init_Stepper_succeed)
        {
            usart0_do();
        }
        if(IORD_ALTERA_AVALON_PIO_DATA(STBUSY_BASE))
            IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE, 1);
        else
            IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE, 0);
        //檢測到有開關(guān)信號(hào)，停止步進(jìn)電機(jī)
        
        stlimit1 = stlimit0;
        stlimit0 = IORD_ALTERA_AVALON_PIO_DATA(STLIMIT_BASE);
        if(stlimit0 != 0 && stlimit1 == 0)  //上升沿
        {
            IOWR_ALTERA_AVALON_PIO_DATA(STSTOP_BASE, 0);
        }
        
        if( Init_PMSM_succeed == 0)
        {
            //if(!IORD_ALTERA_AVALON_PIO_DATA(LIMIT_BASE))
            if(Itimer >= 5)
            {
                Initposition = get_addr();
                locate = Initposition;
                Init_PMSM_succeed = 1;
            }
            else
            {
                locate -= 1000;
            }
        }
        
        
        if(Init_Stepper_succeed == 0)
        {
            if(stlimit0)
            {
                IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 0);
                for(i=0;i<100;i++);
                IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 1);
                for(i=0;i<100;i++);
                IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 0);
                for(i=0;i<100;i++);
                IOWR_ALTERA_AVALON_PIO_DATA(STRST_BASE, 1);
                for(i=0;i<100;i++);
                Init_Stepper_succeed = 1;
            }
        }
        if(timeren10)
        {
            timeren10 = 0;
            disp();
            Itimer++;
        }
    }
    
    return 0;
}