/* 
Main.c
Larry Knight
MainMotion
Revision 1.0 2021
dsPIC33CK512MP608 
*/
#pragma config FNOSC = FRC          // Oscillator Source Selection
#pragma config IESO = ON            // Two-speed Oscillator Start-up Enable

// FOSC
#pragma config POSCMD = EC          // Primary Oscillator Mode Select bits (EC (External Clock) Mode)
#pragma config OSCIOFNC = ON        // OSC2 Pin used for I/O (PMA1)
#pragma config FCKSM = CSECMD       // Clock Switching Mode
#pragma config PLLKEN = ON          // PLL Lock Status Control
#pragma config FWDTEN = ON_SW

#include <xc.h>

#include "MainMotion.h"

bool MotorRun = false;
uint16_t PGtemp;
uint16_t volatile MotorSpeed = 0;
uint8_t volatile Status = 0;
uint8_t volatile Control = 0;
uint8_t volatile _temp;

union bytes2long
{
    uint8_t     byte[4];    
    long        quad;    
};

union bytes2long convert;
union bytes2long distance;
union bytes2long stopDistance;

//number of members in a sequence
const uint8_t sizeOfSeq = 16;

int main()
{
    //oscillator F (20MHz) 
    CLKDIVbits.PLLPRE = 2; // N1=2
    PLLFBDbits.PLLFBDIV = 100; // M = 100
    PLLDIVbits.POST1DIV = 5; // N2=5
    PLLDIVbits.POST2DIV = 1; // N3=1
    
    // Initiate Clock Switch from FRC to PRIPLL
    __builtin_write_OSCCONH(0x03);
    __builtin_write_OSCCONL(OSCCON | 0x01);
    
    // Wait for Clock switch to occur
    while (OSCCONbits.OSWEN!= 0);   
    
    // Wait for PLL to lock
    while (OSCCONbits.LOCK!= 1);    
    
    //Unlock PPS feature
    __builtin_write_RPCON(0);
        
    //Initialize PMP
    dsPIC33_PMP_Init();
    
    //GPIO
    ANSELBbits.ANSELB4 = 0;
    TRISBbits.TRISB4 = 1;

    //Initialize SRAM
    dsPIC33_REN70V05_Init();
        
    //Delay Timer
    Timer1_Init();
    
//    //LED Port
//    TRISDbits.TRISD8 = 0;
//    TRISDbits.TRISD9 = 0;
//    TRISCbits.TRISC9 = 0;
//    TRISCbits.TRISC8 = 0;
//    
//    
//    PORTDbits.RD8 = 0;
//    PORTDbits.RD9 = 0;
//    PORTCbits.RC9 = 0;
//    PORTCbits.RC8 = 0;
//    
    //Initialize PWM
    PWM_Init();
    
    //Write to Board Info Space to indicate presence
    dsPIC33_REN70V05_WR(0, 0x02) ;
    
    dsPIC33_Interrupts_Init();
        
    //QEI1     
    TRISDbits.TRISD13 = 1;  //A
    TRISDbits.TRISD14 = 1;  //B
    TRISDbits.TRISD12 = 1;   //Home
    TRISAbits.TRISA0 = 1;   //Index
  
    ANSELDbits.ANSELD13 = 0;
    
    RPINR14bits.QEIA1R = 77; //A (Pin 14)
    RPINR14bits.QEIB1R = 78; //B (Pin 13)
//    RPINR15bits.QEIHOM1R = 48; //Home (Pin 15)
    RPINR15bits.QEINDX1R = 76; //Index (Pin 27)
    
    QEI1CONbits.QEIEN = 1;
    QEI1CONbits.PIMOD = 0;
    
    /********************/
    /*  Motion Control  */
    /********************/
    //DRV8307 ENABLEn = RE4
    TRISEbits.TRISE4 = 0;
    PORTEbits.RE4 = 1;

    //DRV8307 DIR = RE5
    TRISEbits.TRISE5 = 0;
    PORTEbits.RE5 = 0;

    //DRV8307 BRAKE = RE6
    TRISEbits.TRISE6 = 0;
    PORTEbits.RE6 = 0;

    //DRV8307 LOCKn = RE7
    TRISEbits.TRISE7 = 1;

    //DRV8307 FAULTn = RE8
    TRISEbits.TRISE8 = 1;
    
    //DRV8307 HALLOUT = RE10
    TRISEbits.TRISE10 = 1;
    
    //SRAM test
    //dsPIC33_memtest_70V05();
    
    //Major Revision
    dsPIC33_REN70V05_WR(0x3fc, 0x01) ;
    
    //Minor Revision
    dsPIC33_REN70V05_WR(0x3fd, 0x09) ;

    //Serial Number
    dsPIC33_REN70V05_WR(0x3f7, 0x02) ;

    //Address Key = 0x99, Motion Board is present and ready
    dsPIC33_REN70V05_WR(0x3d2, 0x99) ;
    
    int count_lo;
    int count_hi;
    int count_ex;
    int count_mr;
    
    count_lo = POS1CNTL & 0x0f;
    count_hi = POS1CNTL >> 8;
    count_ex = POS1HLD & 0x0f;
    count_mr = POS1HLD >> 8;
    
    //Capture Register
    uint16_t temp_reg = 0;
    uint8_t extra_reg = 0;
    
    //Clear the position registers in SRAM
    dsPIC33_REN70V05_WR(0x3f0, 0) ;
    dsPIC33_REN70V05_WR(0x3f1, 0) ;
    dsPIC33_REN70V05_WR(0x3f2, 0) ;
    dsPIC33_REN70V05_WR(0x3f3, 0) ;
    
    //set the defaults
    //Status Ready
    Status = 0x01;
    dsPIC33_REN70V05_WR(0x3fe, Status) ;

    //Speed
    dsPIC33_REN70V05_WR(0x3f8, 100) ;
    
    //Acceleration
    dsPIC33_REN70V05_WR(0x3fa, 50) ;
    
    //Deceleration
    dsPIC33_REN70V05_WR(0x3f9, 50) ;
    
    //Control Byte
    dsPIC33_REN70V05_WR(0x3ff, 0) ;
    
//    //Set the QEI status Register
//    QEI1STAT = 1 << 12;
    
    //Get the end position from SRAM
    dsPIC33_REN70V05_RD(0x303);
    extra_reg = mdata_70V05;
    temp_reg = temp_reg + extra_reg;
    temp_reg = extra_reg << 8;
    dsPIC33_REN70V05_RD(0x302);
    extra_reg = mdata_70V05;
    QEI1GECL = temp_reg + extra_reg;

    dsPIC33_REN70V05_RD(0x305);
    extra_reg = mdata_70V05;
    temp_reg = temp_reg + extra_reg;
    temp_reg = extra_reg << 8;
    dsPIC33_REN70V05_RD(0x304);
    extra_reg = mdata_70V05;
    QEI1GECH = temp_reg + extra_reg;
    
    //write current PG4PER value (16 bits)
    PGtemp = PG4PER & 0x00ff;
    dsPIC33_REN70V05_WR(0x3de, PGtemp) ;
    PGtemp = PG4PER & 0xff00;
    PGtemp = PGtemp >> 8;
    dsPIC33_REN70V05_WR(0x3df, PGtemp) ;
    
    INT1TMRH = 0x0000;
    INT1TMRL = 0xff00;
    
    //Initialize the SRAM for Sequences
    int i;
    for(i=0x300;i<0x390;i++)
    {
        dsPIC33_REN70V05_WR(i, 00);
    }
    
    //Set Sequence default data
    dsPIC33_REN70V05_WR(0x300, 01);
    
    
    uint8_t test = 0;
    
    while(1)
    {
        //comm check token
        test = dsPIC33_REN70V05_RD(0x3d3);
        test = test++;
        dsPIC33_REN70V05_WR(0x3d4, test);

        //Read SRAM for Control byte
        dsPIC33_REN70V05_RD(0x3ff);
        Control = mdata_70V05;

        //Read SRAM for Status byte
        dsPIC33_REN70V05_RD(0x3fe);
        Status = mdata_70V05;

         //This is what we do normally
        if((Control & 0x80) == 0)
        {
            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;

            //Read SRAM for Status byte
            dsPIC33_REN70V05_RD(0x3fe);
            Status = mdata_70V05;
            
            //check for drive Fault
            //DRV8307 FAULTn = RE8
            if(PORTEbits.RE8 == 0)
            {
                Status = (Status | 0x40);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }
            else
            {
                Status = (Status & 0xbf);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }
      

            //check for drive Lock
            //DRV8307 LOCKn = RE7
            if(PORTEbits.RE7 == 0)
            {
                Status = (Status | 0x80);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }
            else
            {
                Status = (Status & 0x7f);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }

            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;
            
            //RUN - Acceleration
            if((Control & 0x81) == 0x01 && (Status & 0x03) == 1)
            {
                Motor_Run();
            }

            //RUN - Full Speed
            if((Control & 0x01) == 0x01 && (Status & 0x23) == 0x23)
            {
                //Motor speed set
                dsPIC33_REN70V05_RD(0x3f8);
                MotorSpeed = mdata_70V05;
                MotorSpeed = MotorSpeed * 16;

                while(!PG4STATbits.UPDATE)                
                {
                    PG4DC = MotorSpeed;
                }   
            }

            //clear count
            if((Control & 0x04) == 0x04)
            {
                Clear_Count();
            }

            //CW
            if((Control & 0x02) == 0x02)
            {
                QEI1STAT = 0x0400;
                //DIR
                PORTEbits.RE5 = 1;
                //write Status
                Status = Status | 0x04;
                dsPIC33_REN70V05_WR(0x3f5, Status) ;
            }

            //CCW
            if((Control & 0x02) == 0x00)
            {
                QEI1STAT = 0x1000;
                PORTEbits.RE5 = 0;
                Status = Status & 0xfb;
                dsPIC33_REN70V05_WR(0x3f5, Status) ;
            }        

            //read QEI1STAT and load it into sram upper 16 bits = 0
            char a = 0;

            a = QEI1STAT;
            dsPIC33_REN70V05_WR(0x3dc, a) ;

            a = QEI1STAT >> 8;
            dsPIC33_REN70V05_WR(0x3dd, a) ;  

            //Write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();

            //STOP
            if((Control & 0x81) == 0x00 && (Status & 0x03) == 0x03)
            {
                Motor_Stop();
            }

            if((Control & 0x81) == 0x80 && (Status & 0x03) == 0x03)
            {
                Motor_Stop();
            }

            //Update SRAM with Duty Cycle
            dsPIC33_REN70V05_WR(0x3d6, PG4DC) ;  
            dsPIC33_REN70V05_WR(0x3d7, PG4DC >> 8) ;  

            Delay(10000);
        }
        
        /********************************************************************/
        /*               This is what we do in Sequence Mode                */
        /********************************************************************/
        if((Control & 0x80) == 0x80)
        {
            //Read SRAM for Status byte
            dsPIC33_REN70V05_RD(0x3fe);
            Status = mdata_70V05;

            //check for drive Fault
            //DRV8307 FAULTn = RE8
            if(PORTEbits.RE8 == 0)
            {
                Status = (Status | 0x40);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }
            else
            {
                Status = (Status & 0xbf);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }

            //check for drive Lock
            //DRV8307 LOCKn = RE7
            if(PORTEbits.RE7 == 0)
            {
                Status = (Status | 0x80);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }
            else
            {
                Status = (Status & 0x7f);
                dsPIC33_REN70V05_WR(0x3fe, Status) ;
            }

            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;

            //SEQ RUN
            if((Control & 0x81) == 0x81 && (Status & 0x01) == 1)
            {
                //Get the delay between sequences
                dsPIC33_REN70V05_RD(0x30d);
                uint8_t seqDelay = mdata_70V05;
                seqDelay = seqDelay * 1000;

                int i;
                //Get the total sequences to run
                dsPIC33_REN70V05_RD(0x30e);
                uint8_t total = mdata_70V05;
                
                uint8_t seqLoop = 0;
                //Get the seqLoop value from Control bit D4
                dsPIC33_REN70V05_RD(0x30f);
                seqLoop = mdata_70V05;
                               
                if(seqLoop > 0)
                {
                    while(1)
                    {
                        for(i=0;i<=total;i++)
                        {
                            SEQ_Run(i);

                            //Stopped - must clear acceleration done bit
                            Status = Status & 0xd7;
                            dsPIC33_REN70V05_WR(0x3fe, Status) ;
                            
                            Delay(seqDelay);
                        }
                    }
                }
                else
                {
                    for(i=0;i<=total;i++)
                    {
                        SEQ_Run(i);

                        //Stopped - must clear acceleration done bit
                        Status = Status & 0xd7;
                        dsPIC33_REN70V05_WR(0x3fe, Status) ;
                    }
                    
                    //Delay(seqDelay);
                }
                
               Motor_Stop();
            }

            //read QEI1STAT and load it into sram upper 16 bits = 0
            char a = 0;

            a = QEI1STAT;
            dsPIC33_REN70V05_WR(0x3dc, a) ;

            a = QEI1STAT >> 8;
            dsPIC33_REN70V05_WR(0x3dd, a) ;  

            //Write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();

            //STOP
            if((Control & 0x81) == 0x00 && (Status & 0x03) == 0x03)
            {
                Motor_Stop();
            }

            if((Control & 0x81) == 0x80 && (Status & 0x03) == 0x03)
            {
                Motor_Stop();
            }

            //Update SRAM with Duty Cycle
            dsPIC33_REN70V05_WR(0x3d6, PG4DC) ;  
            dsPIC33_REN70V05_WR(0x3d7, PG4DC >> 8) ;  

            //Delay(1000);
        }      
    }       
}

void WritePosSRAM(void)
{
    //write position to SRAM
    dsPIC33_REN70V05_WR(0x3f0, POS1CNTL) ;
    dsPIC33_REN70V05_WR(0x3f1, POS1CNTL >> 8) ;
    dsPIC33_REN70V05_WR(0x3f2, POS1HLD) ;    
    dsPIC33_REN70V05_WR(0x3f3, POS1HLD >> 8) ;
    
    //Create the 32 bit value
    convert.byte[0] = POS1CNTL;
    convert.byte[1] = POS1CNTL >> 8;
    convert.byte[2] = POS1HLD;
    convert.byte[3] = POS1HLD >> 8;  
}

void WriteIndexSRAM(void)
{
        //Write the Index Count Register to SRAM
        dsPIC33_REN70V05_WR(0x3eb, INDX1CNTL) ;
        dsPIC33_REN70V05_WR(0x3ec, INDX1CNTL >> 8) ;
        dsPIC33_REN70V05_WR(0x3ed, INDX1HLD) ;
        dsPIC33_REN70V05_WR(0x3ee, INDX1HLD >> 8) ;      
}

void Clear_Count(void)
{
    //QEI1STAT = 0x1000;
    POS1HLD = 0;
    POS1CNTL = 0;
    INDX1HLD = 0;
    INDX1CNTL = 0;
    
    //write position to SRAM
    WritePosSRAM();
    
    Control = (Control & 0xfb);
    dsPIC33_REN70V05_WR(0x3ff, Control) ;
}

void Motor_Home(void)
{

    Status = 1;
    dsPIC33_REN70V05_WR(0x3f5, Status) ;
    Motor_Run();
    while(PORTDbits.RD12);
    Motor_Stop();

    Status = 0 << 4;
    dsPIC33_REN70V05_WR(0x3f5, Status) ;
}

void Motor_Run(void)
{
    int i;
    int dly = 500;
    
    //DRV8307 Brake
    PORTEbits.RE6 = 0;

    //DRV8307 ENABLEn
    PORTEbits.RE4 = 0;
    
    //get acceleration
    dsPIC33_REN70V05_RD(0x3fa);
    dly = mdata_70V05;
    
    dly = dly * 30;

    //Motor speed set
    dsPIC33_REN70V05_RD(0x3f8);   
    MotorSpeed = mdata_70V05;
    MotorSpeed = MotorSpeed * 16;
    
    //Running
    Status = Status | 0x02;
    dsPIC33_REN70V05_WR(0x3fe, Status) ; 
    
    //Acceleration done?
    if((Status & 0x20) == 0)
    {               
        for(i=0x240;i<=MotorSpeed;i++)
        {
            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;
        
            if((Control & 0x01) == 0)
            {
                Motor_Stop();
                return;
            }

            while(!PG4STATbits.UPDATE)
            {
                PG4DC = i;
                //Update SRAM with Duty Cycle
                dsPIC33_REN70V05_WR(0x3d6, PG4DC) ;  
                dsPIC33_REN70V05_WR(0x3d7, PG4DC >> 8) ;  
 
            }

            Delay(dly);

            //update SRAM
            dsPIC33_REN70V05_WR(0x3d5, (PG4DC / 16)) ;

            //write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();
        }
        
        //Acceleration done
        Status = Status | 0x20;
        dsPIC33_REN70V05_WR(0x3fe, Status) ;
    }  
    
}

void SEQ_Run(uint8_t seqNum)
{
    int i;
    int dly = 500;
    
    //calculate the address
    
    //Get the sequence number
    dsPIC33_REN70V05_RD(0x300);
    if(mdata_70V05 == 0xff)
    {
        Motor_Stop();
    }
    //Get the Distance value
    dsPIC33_REN70V05_RD(0x302 + (sizeOfSeq * seqNum));
    distance.byte[0] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x303 + (sizeOfSeq * seqNum));
    distance.byte[1] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x304 + (sizeOfSeq * seqNum));
    distance.byte[2] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x305 + (sizeOfSeq * seqNum));
    distance.byte[3] = mdata_70V05;

    //Get the Stop Distance value
    dsPIC33_REN70V05_RD(0x309 + (sizeOfSeq * seqNum));
    stopDistance.byte[0] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x30a + (sizeOfSeq * seqNum));
    stopDistance.byte[1] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x30b + (sizeOfSeq * seqNum));
    stopDistance.byte[2] = mdata_70V05;
    dsPIC33_REN70V05_RD(0x30c + (sizeOfSeq * seqNum));
    stopDistance.byte[3] = mdata_70V05;

    //Get direction
    dsPIC33_REN70V05_RD(0x307 + (sizeOfSeq * seqNum));
    //CCW
    if(mdata_70V05)
    {
        PORTEbits.RE5 = 1;
        //distance.quad = ~distance.quad;
        //stopDistance.quad = ~stopDistance.quad;
        QEI1STAT = 0x0400;
        Status = Status | 0x04;
        dsPIC33_REN70V05_WR(0x3f5, Status) ;
    }
    else
    {
    //CW
        PORTEbits.RE5 = 0;
        QEI1STAT = 0x1000;
        Status = Status & 0xfb;
        dsPIC33_REN70V05_WR(0x3f5, Status) ;
    }

    //get acceleration
    dsPIC33_REN70V05_RD(0x306 + (sizeOfSeq * seqNum));
    dly = mdata_70V05;
    dly = dly * 30;

    //Get Motor speed
    dsPIC33_REN70V05_RD(0x308 + (sizeOfSeq * seqNum));   
    MotorSpeed = mdata_70V05;
    MotorSpeed = MotorSpeed * 16;
    
    //DRV8307 Brake
    PORTEbits.RE6 = 0;

    //DRV8307 ENABLEn
    PORTEbits.RE4 = 0;

    Clear_Count();
    //Read SRAM for Control byte
    dsPIC33_REN70V05_RD(0x3ff);
    Control = mdata_70V05;

    //Running
    Status = Status | 0x02;
    dsPIC33_REN70V05_WR(0x3fe, Status) ; 

    //Acceleration done?
    if((Status & 0x20) == 0)
    {               
        for(i=0x240;i<=MotorSpeed;i++)
        {
            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;

            if((Control & 0x01) == 0)
            {
                Motor_Stop();
                return;
            }

            while(!PG4STATbits.UPDATE)
            {
                PG4DC = i;
                //Update SRAM with Duty Cycle
                dsPIC33_REN70V05_WR(0x3d6, PG4DC) ;  
                dsPIC33_REN70V05_WR(0x3d7, PG4DC >> 8) ;  

            }

            Delay(dly);

            //update SRAM
            dsPIC33_REN70V05_WR(0x3d5, (PG4DC / 16)) ;

            //write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();
        }
    }
    //Acceleration done
    Status = Status | 0x20;
    dsPIC33_REN70V05_WR(0x3fe, Status) ;

    //now we run until counter = runto value
    //RUN - Full Speed
    if(!PORTEbits.RE5)
    {
        while(convert.quad < distance.quad)
        {
            if((Control & 0x01) == 0x01 && (Status & 0x23) == 0x23)
            {
                //Motor speed set
                dsPIC33_REN70V05_RD(0x308 + (sizeOfSeq * seqNum));
                MotorSpeed = mdata_70V05;
                MotorSpeed = MotorSpeed * 16;

                while(!PG4STATbits.UPDATE)                
                {
                    PG4DC = MotorSpeed;
                }   
            }

            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;
            if((Control & 0x01) == 0)
            {
                Motor_Stop();
                return;
            }

            //write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();
        }
    }
    else
    {
        while(convert.quad > distance.quad)
        {
            if((Control & 0x01) == 0x01 && (Status & 0x23) == 0x23)
            {
                //Motor speed set
                dsPIC33_REN70V05_RD(0x308 + (sizeOfSeq * seqNum));
                MotorSpeed = mdata_70V05;
                MotorSpeed = MotorSpeed * 16;

                while(!PG4STATbits.UPDATE)                
                {
                    PG4DC = MotorSpeed;
                }   
            }

            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;
            if((Control & 0x01) == 0)
            {
                Motor_Stop();
                return;
            }

            //write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();
        }
    }
    //Now decelerate
    //get deceleration
        dsPIC33_REN70V05_RD(0x301 + (sizeOfSeq * seqNum));
        dly = mdata_70V05;
        dly = dly * 30;

        for(i=MotorSpeed;i>=0x240;i--)
        {
            //Read SRAM for Control byte
            dsPIC33_REN70V05_RD(0x3ff);
            Control = mdata_70V05;

            if((Control & 0x01) == 0)
            {
                Motor_Stop();
                return;
            }

            while(!PG4STATbits.UPDATE)
            {
                PG4DC = i;
                //Update SRAM with Duty Cycle
                dsPIC33_REN70V05_WR(0x3d6, PG4DC) ;  
                dsPIC33_REN70V05_WR(0x3d7, PG4DC >> 8) ;  
            }

            Delay(dly);

            //update SRAM 
            dsPIC33_REN70V05_WR(0x3d5, (PG4DC / 16)) ;

            //write position to SRAM
            WritePosSRAM();
            WriteIndexSRAM();

            if(!PORTEbits.RE5)
            {
                if(convert.quad > stopDistance.quad)
                {
                    Seq_Stop();
                    return;
                } 
            }
            else
            {
                if(convert.quad < stopDistance.quad)
                {
                    Seq_Stop();
                    return;
                } 

            }
        }   

    Seq_Stop();  
}

void Seq_Stop(void)
{
    while(!PG4STATbits.UPDATE)
    {
        PG4DC = 0;
    }
    //dsPIC33_REN70V05_WR(0x3f8, 0) ;
    
    //Brake
    PORTEbits.RE6 = 1;
    
    //Enable
    PORTEbits.RE4 = 1;
    
    //update PG4DC / 16 register
    dsPIC33_REN70V05_WR(0x3d5, (PG4DC / 16)) ;
}

void Motor_Stop(void)
{
    while(!PG4STATbits.UPDATE)
    {
        PG4DC = 0;
    }
    //dsPIC33_REN70V05_WR(0x3f8, 0) ;
    
    //Brake
    PORTEbits.RE6 = 1;
    
    //Enable
    PORTEbits.RE4 = 1;
    
    //update PG4DC / 16 register
    dsPIC33_REN70V05_WR(0x3d5, (PG4DC / 16)) ;
    
    //write command byte
    Control = (Control & 0xfe);
    dsPIC33_REN70V05_WR(0x3ff, Control) ;
    
    //Stopped
    Status = (Status & 0x01);
    dsPIC33_REN70V05_WR(0x3fe, Status) ;
}