Dxl.c

Go to the documentation of this file.

#include    <windows.h>
#include    <stdio.h>
#include    <math.h>
#include "mgui.h"
#include "DAS_Spatram.h"
#include "dcl.h"
#include "dil.h"
#include "bil.h"
#include "dscud.h"
#include "dxl.h"
#include "wsc.h"


DXL_par DXL;
//cpl CPL;          //Control Panel
flag FLAG; //flags structure
//stepm STEPM;      //Stepper Motors
das DAS;

/* 1) Assign the AMSx Address */
void AMS_assadd(int w)
{
    DXL.add = w;
}

/* 2) Test positioning */
int AMS_TestPos(int id, int motor)
{
//  char buf[80];
    int d, er; 
    long ns;
    //Assign Address
    AMS_assadd(id);

//  if (steps > period)
//  {
//      nv = steps / period;
//  }
//  k = 1;
    do
    {
//      if(k) 

        sprintf(DXL.buftx, "8%02X", motor);
        DXL_TxStr(DXL.buftx, 3);
        d = STS0LEN;
        er = DXL_RxStr(DXL.bufrx, &d);
        ns = AMS_ReadStep(id, motor);
        MLoopWhileEvents(0);
    //}while(DXL.bufrx[0] == '0' && DXL.bufrx[1] == '0' && er == 0);
    }while(DXL.bufrx[0] == '0' && DXL.bufrx[1] == '0' && er == 0  && ns != 0);

    return er;
}
/* 3) Read motor step */
unsigned long AMS_ReadStep(int id, int motor)
{

    char buf[80];
    int d, er; 
    long ns;
    
    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "1%02X", motor);
    DXL_TxStr(DXL.buftx, 3);
    d = 15;
    er = DXL_RxStr(buf, &d);
    if(er == 0)
    {
        sscanf(buf, "%lx", &ns);
        return ns;
    }
    
    return 0;
}

unsigned char DXL_HexBin(char *str)
{
    unsigned char   ch;
    if(str[0] <= '9')
        ch = (str[0] - '0') << 4;
    else
        ch = (str[0] - '0' - 7) << 4;
    if(str[1] <= '9')
        ch += str[1] - '0';
    else
        ch += str[1] - '0' - 7;
    return ch;
}

/*  4) AMS_chkbitsts = CHECK BIT STATUS of the AMS Input Port (IOP(i) with i=0..7)
    INPUT PARAMETERS: 
    id:   AMS ADDRESS
    bitn: BIT NUMBER --> 0 = LSB, 7 = MSB

    OUTPUT:
    sts:  BIT STATUS --> sts = 0 --> bitn = 0, sts = 1 --> bitn = 1
*/
int AMS_chkbitsts_old(int id, int bitn)
{


    int bn = bitn;
    int sts = 2;
    char    str[80], buf[80], *s;
    int d, er; 
    unsigned long stbyte = 0;   

    //Assign Address
    AMS_assadd(id);

    //Read AMS Input
    sprintf(DXL.buftx, "/");
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = STS0LEN;
    er = DXL_RxStr(buf, &d);

    DXL.bufrx[0] = '0';
    DXL.bufrx[1] = '0';
    er = 0;
    sprintf(str,"0x%s",buf);
    stbyte = strtol(str, &s, 16);   

    switch (bn)
    {
        case 0:
            sts = stbyte & 1;
            sts = sts >> bn;
            break;
        case 1:
            sts = stbyte & 2;
            sts = sts >> bn;
            break;
        case 2:
            sts = stbyte & 4;
            sts = sts >> bn;
            break;
        case 3:
            sts = stbyte & 8;
            sts = sts >> bn;
            break;
        case 4:
            sts = stbyte & 16;
            sts = sts >> bn;
            break;
        case 5:
            sts = stbyte & 32;
            sts = sts >> bn;
            break;
        case 6:
            sts = stbyte & 64;
            sts = sts >> bn;
            break;
        case 7:
            sts = stbyte & 128;
            sts = sts >> bn;
            break;
    }
    
    if (sts == 2)
        return 2;
    else
        return sts;

}

/* 5) Assign default parameters */
void AMS_Default(int id)
{

    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Set default values
    DXL_TxStr("A", 1);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);


}


int AMS_chkbitsts(int id, int bitn)
{


    int sts = 2;
//  char    str[80], buf[80], *s;
    int d, er; 
    unsigned long stbyte = 0;
    unsigned char a;
    
//  buf[0] = 0;
//  buf[1] = 0;
    //Assign Address
    AMS_assadd(id);

    //Read AMS Input
//  delay (70);
    sprintf(DXL.buftx, "/");
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = STS0LEN;
//  delay (70);
    er = DXL_RxStr(DXL.bufrx, &d);
//  er = DXL_RxStr(buf, &d);

    if(er) Message("Serial Error!", er);
    else
    {
        a = DXL_HexBin(DXL.bufrx);

//      sprintf(buf, "%d", a);
//      MObjectSetText(CONFP.lblStatus, buf);
//      if (a = 100)
//          er = DXL_RxStr(buf, &d);
//          a = DXL_HexBin(buf);


    
        switch (bitn)
        {
            case 0:
                sts = (a >> 0) & 1;
                break;
            case 1:
                sts = (a >> 1) & 1;
                break;
            case 2:
                sts = (a >> 2) & 1;
                break;
            case 3:
                sts = (a >> 3) & 1;
                break;
            case 4:
                sts = (a >> 4) & 1;
                break;
            case 5:
                sts = (a >> 5) & 1;
                break;
            case 6:
                sts = (a >> 6) & 1;
                break;
            case 7:
                sts = (a >> 7) & 1;
                break;
        
        
        
        
        }
        
        
        
        
//      sprintf(buf, "A Proxy, Home = %d, Work = %d\nB Proxy, Home = %d, Work = %d\nAll bits = %02XH, %d%d%d%d%d%d%d%d bin",
//              (a >> 3) & 1, (a >> 2) & 1, (a >> 1) & 1, a & 1, a,
//              (a >> 7) & 1, (a >> 6) & 1, (a >> 5) & 1, (a >> 4) & 1,(a >> 3) & 1, (a >> 2) & 1, (a >> 1) & 1, a & 1, a & 0);
//      Message(buf, er);

  }

    
    
    
//  sprintf(str,"0x%s",buf);
//  sprintf(str,"0x%s",DXL.bufrx);
//  stbyte = strtol(str, &s, 16);   
/*
    switch (bn)
    {
        case 0:
            sts = stbyte & 1;
            sts = sts >> bn;
            break;
        case 1:
            sts = stbyte & 2;
            sts = sts >> bn;
            break;
        case 2:
            sts = stbyte & 4;
            sts = sts >> bn;
            break;
        case 3:
            sts = stbyte & 8;
            sts = sts >> bn;
            break;
        case 4:
            sts = stbyte & 16;
            sts = sts >> bn;
            break;
        case 5:
            sts = stbyte & 32;
            sts = sts >> bn;
            break;
        case 6:
            sts = stbyte & 64;
            sts = sts >> bn;
            break;
        case 7:
            sts = stbyte & 128;
            sts = sts >> bn;
            break;
    }
    
    if (sts == 2)
        return 2;
    else

*/
        return sts;

}



/* 6)  */
int AMS_Power(int id, int powa, int powb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "&%02X%02X", 0, 0);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);
    delay(50);
    //send power
    sprintf(DXL.buftx, "&%02X%02X", powa, powb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

/* 7)  */
int AMS_Step(int id, int stepa, int stepb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Send step
    sprintf(DXL.buftx, "'%02X%02X", stepa, stepb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;

}

/* 8)  */
int AMS_Slope(int id, int slopea, int slopeb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Send slope
    sprintf(DXL.buftx, "(%02X%02X", slopea, slopeb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

/* 9)  */
int AMS_Speed(int id, int speeda, int speedb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Send speed
    sprintf(DXL.buftx, "4%02X%02X", speeda, speedb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

/* 10)  */
int AMS_TrackOn(int id, int adir, int bdir, unsigned long acount, unsigned long bcount)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "0%02X%08lX%02X%08lX", adir, acount, bdir, bcount);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);
    
    return er;

}

/* 11)  */
int AMS_TrackOff(int id, int amot, int bmot)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "5%02X%02X", amot, bmot);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;

}
/* 12)  */
int AMS_AzzeraCoord(int id, int mtr)
{
    int er, d;
    char buf[40];

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "3%02X", mtr);
    DXL_TxStr(DXL.buftx,3);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);    
    
    return er;

}

/* 13)  */
int AMS_dirfin(int id, int dira, int dirb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Set final direction
    sprintf(DXL.buftx, "-%02X%02X", dira, dirb);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

/* 14)  */
int AMS_firmrev(int id)
{
//  char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Firmware Revision
    DXL_TxStr("%%", 1);
    d = STS0LEN;
    er = DXL_RxStr(DXL.bufrx, &d);

    return er;
}

/* 15)  */
void AMS_PWM(int id, int on, int off)
{
    char buf[80];
    int er, d;

    //Assign Address
    AMS_assadd(id);

//  on = 450;
//  off = 50;
    sprintf(DXL.buftx, "6%08X%08X", (unsigned int)on,  (unsigned int)off );
    DXL.add = 1; //AMS2 address
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    delay(500);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);    

}


/* 16)  */
int AMS_sendstep(int id, int dira, int dirb, unsigned long stepa, unsigned long stepb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    //Send  step number
    sprintf(DXL.buftx,".%02X%08lX%02X%08lX", dira, stepa, dirb, stepb);
    DXL_TxStr(DXL.buftx, 21);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}
/* 17)  */
int AMS_SetPolarity(int id, int homea, int worka, int homeb, int workb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    //Send Polarity for home and work
    sprintf(DXL.buftx,"*%02X%02X%02X%02X", homea, worka, homeb, workb);
    DXL_TxStr(DXL.buftx, 21);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}


/* 18)  */
int AMS_Home(int id, int mota, int motb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    //Send Polarity for home and work
    sprintf(DXL.buftx,"+%02X%02X", mota, motb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

/* 19)  */
int AMS_HomeDefault(int id, int mota, int motb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    //Send Polarity for home and work
    sprintf(DXL.buftx,"<%02X%02X", mota, motb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}

void AMS_PWM_OFF(int id)
{
    char buf[80];
    int er, d;

    //Assign Address
    AMS_assadd(id);

//  on = 450;
//  off = 50;
    sprintf(DXL.buftx, "7", 1);
    DXL.add = 1; //AMS2 address
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);    

}






void D_Positioning(MOBJECT p, int id, long tout)
{
    unsigned long   st, et, x;
    long tl;
    char buf[80];

    if (DAS.Flag.exemode == DBDEMO)
        tout = 1000;
    
    st = GetTickCount();
    do
    {
        et = GetTickCount();
        x = labs(et - st);
        //g = x % 10;
        //sprintf(buf, " %u %u", x, g);
        //Status (buf);
        //MLoopWhileEvents(0);
            //if((x % 10 == 0) && (x > 0))
            if(x > 0)
            {
                if(DAS.DOption.amsprogbar)
                {
                    D_VRange(NULL, (long) x * 100 / tout, 255, 0, 0 );
                    tl = (long) ((double)fabs(((double)(labs(et - st) - tout))/1000.0 ));
                    sprintf(buf, "%.1lf sec. left", (double)fabs(((double)(labs(et - st) - tout))/1000.0 ));
                    MObjectSetText(p, buf);
                    DAS.StepM.postime[AMS1][DAS.StepM.SM_B]=(unsigned long)(tl*1000);

                }
            }
    }while(labs(et - st) < tout);

}


/* 20)  */
int AMS_DoPos(int id, int dira, int dirb, unsigned long stepa, unsigned long stepb)
{
    int     er = 0;
//  unsigned long x;
    long ns, cont;
    char buf[64];

    FlgSM_Stop = 0;

    //Assign Address
    AMS_assadd(id);


    if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
    {

        // Testa se il precedente posizionamento e' finito
        if(stepa)
            er = AMS_TestPos(id, 0);
        if(er) return er;

        // Testa se il precedente posizionamento e' finito
        if(stepb)
            er = AMS_TestPos(id, 1);
        if(er) return er;
    }

/*********************************/
        
    if(stepa && id == 0)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning Mirror",0, 255, 0, 0);
    if(stepb && id == 0)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning Grating",0, 255, 255, 0);
    if(stepa && id == 1)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning Filter Wheel",0, 0, 0, 255);
    if(stepb && id == 1)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning ND Filter Wheel",0, 192, 0, 255);
    if(stepa && id == 2)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning Zenith",0, 0, 255, 255);
    if(stepb && id == 2)
        if(DAS.DOption.amsprogbar)
            D_VRange("Positioning Azimuth",0, 0, 192, 255);

/*********************************/


    // Trasmette numero di passi
    if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
        er = AMS_sendstep(id, dira, dirb, stepa, stepb);

    
    if(stepa && id == 0)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            cont = (long) (stepa / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_A]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[0], 0, cont);
    }
    if(stepb && id == 0)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            DAS.StepM.postime[id][DAS.StepM.SM_B] = (long) (stepb / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_B]);
            //cont = (long) (stepb / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_B]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[1], 1, DAS.StepM.postime[id][DAS.StepM.SM_B]);
        //D_Positioning(DAS.Gui.ContrPanel.Albl_LS[1], 1, cont);
    }
    if(stepa && id == 1)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            cont = (long) (stepa / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_A]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[2], 2, cont);
    }       
    if(stepb && id == 1)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            cont = (long) (stepb / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_B]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[3], 3, cont);
    }       
    if(stepa && id == 2)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            cont = (long) (stepa / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_A]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[4], 4, cont);
    }       
    if(stepb && id == 2)
    {
        if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
            cont = (long) (stepb / (float) DAS.StepM.speed_Hz[id][DAS.StepM.SM_B]);
        D_Positioning(DAS.Gui.ContrPanel.Albl_LS[5], 5, cont);
    }       



    if(DAS.DOption.amsprogbar)
        D_VRange(NULL, 100, 0,0,0);

/*********************************/
    /* FILE DI LOG */   
    if(stepa && id == 0)
        writelogmotor(0, 0, er, dira, stepa, 0);
    if(stepb && id == 0)
        writelogmotor(0, 1, er, dirb, stepb, 0);
    if(stepa && id == 1)
        writelogmotor(0, 2, er, dira, stepa, 1);
    if(stepb && id == 1)
        writelogmotor(0, 3, er, dirb, stepb, 1);
    if(stepa && id == 2)
        writelogmotor(0, 4, er, dira, stepa, 2);
    if(stepb && id == 2)
        writelogmotor(0, 5, er, dirb, stepb, 2);
/*********************************/

    if(er) return er;

    if ((DAS.Flag.exemode != DBDEMO) & (DAS.Flag.exemode != TESTSZAMODE))
    {
        // Testa se il precedente posizionamento e' finito
        if(stepa)
            er = AMS_TestPos(id, 0);
        if(er) return er;

        // Testa se il precedente posizionamento e' finito
        if(stepb)
            er = AMS_TestPos(id, 1);
        if(er) return er;

        /*******************************************************/
        /*  Conta numero di passi effettuati */
        if(stepa)
        {
            ns = AMS_ReadStep(id, 0);
            sprintf(buf, "Steps A: %ld", ns);
            Status(buf);
        }

        if(stepb)
        {
            ns = AMS_ReadStep(id, 1);
            sprintf(buf, "Steps B: %ld", ns);
            Status(buf);
        }

        /******************************************************/                
    }
    else //DEMO MODE
    {
        /*  Conta numero di passi effettuati */
        if(stepa)
        {
            
            sprintf(buf, "Steps A: %ld", stepa);
            Status(buf);
        }

        if(stepb)
        {
            
            sprintf(buf, "Steps B: %ld", stepb);
            Status(buf);
        }
    
    }

    FlgSM_Stop = 1;
    return er;  
}


int DB_StepMotor(int id, int motor, long step, int dir)
{
    
    char buf[80];
//  char Dbuf[80];
    int er = 0;
    int c = motor, d = 0;

    

    switch(c)
    {
        //INPUT MIRROR
        case 0: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][0], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[0], buf);
                DXL.add = 0;
                er = AMS_DoPos(id, dir, 0, step, 0);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)  { Message("n Step not sent! 1", er); break; }
                break;
                
        //GRATING
        case 1: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][1], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[1], buf);
                DXL.add = 0;
                er = AMS_DoPos(id, 0, dir, 0, step);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)  { Message("n Step not sent! 2", er); break; }
                break;

        //FILTER WHEEL
        case 2: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][0], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[2], buf);     

                DXL.add = 1;
                er = AMS_DoPos(id, dir, 0, step, 0);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)  { Message("n Step not sent! 2", er); break; }
                DXL.add = 0;
                break;

        // ND FILTER WHEEL
        case 3: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][1], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[3], buf);     

                DXL.add = 1;
                er = AMS_DoPos(id, 0, dir, 0, step);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)  { Message("n Step not sent! 2", er); break; }
                DXL.add = 0;
                break;
                

        //Zenith
        case 4: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][0], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[4], buf);     

                DXL.add = 2;
                er = AMS_DoPos(id, dir, 0, step, 0);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)
                        { 
                            Message("n Step not sent! 3", er);
                            break;
                        }
                DXL.add = 0;
                break;
        //Azimuth
        case 5: MPixmapSetImageFile(DAS.Gui.ContrPanel.LedSM[id][1], "SYS\\ledoff.bmp");
                sprintf(buf, "%s", "Positioning");
                MObjectSetText(DAS.Gui.ContrPanel.Albl_LS[5], buf);     

                DXL.add = 2;
                er = AMS_DoPos(id, 0, dir, 0, step);
                    if (DAS.Flag.exemode != DBDEMO)
                        if(er)  { Message("n Step not sent! 4", er); break; }
                DXL.add = 0;
                break;

    
    }
    

    return 1;   
}




/* 21) */
int AMS_wopto(int id, unsigned int w)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);
    //Write the opto port
    sprintf(DXL.buftx, "9%02X", w); 
    DXL_TxStr(DXL.buftx, 3);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);    

    return er;  
}

//mtr1(or 2) = 0 --> motor off
//mtr1(or 2) = 1 --> motor on
/* 22 */
int AMS_MotorOn_Off(int id, int mtr1, int mtr2)
{
    char    buf[80];
    int     er = 0, d;

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx, "2%02X%02X", mtr1, mtr2);
    DXL_TxStr(DXL.buftx,strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);    
    return er;
}

/*  23  */
int AMS_Divisore(int id, unsigned long par1, unsigned long par2)
{

    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    //set divisore
    sprintf(DXL.buftx,">%08lX%08lX", par1, par2);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx)); 
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;

}

/* 23)  */
int AMS_ESlope(int id, int eslopea, int eslopeb)
{
    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Send slope
    sprintf(DXL.buftx, ";%02X%02X", eslopea, eslopeb);
    DXL_TxStr(DXL.buftx, 5);
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}



/* 24  */
int AMS_Proxy(int id)
{

    char buf[128];
    int d, er; 
    unsigned char a;
    //Assign Address
    AMS_assadd(id);

    DXL_TxStr("/", 1);

    d = STS0LEN;
    er = DXL_RxStr(buf, &d);
    if(er) Message("Serial Error!", er);
    else
    {
        a = DXL_HexBin(buf);
        sprintf(buf, "A Proxy, Home = %d, Work = %d\nB Proxy, Home = %d, Work = %d\nAll bits = %02XH, %d%d%d%d%d%d%d%d bin",
                (a >> 3) & 1, (a >> 2) & 1, (a >> 1) & 1, a & 1, a,
                (a >> 7) & 1,(a >> 6) & 1, (a >> 5) & 1, (a >> 4) & 1,(a >> 3) & 1, (a >> 2) & 1, (a >> 1) & 1, a & 1 );
        Message(buf, er);
    }
    
    return er;
}

/*  25  */
int AMS_ReadAD(int id, int ch)
{

    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    // Send slope
    sprintf(DXL.buftx, ":%02X", ch );
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx));
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;
}



/*   27   */
int AMS_TestHome(int id, int par1)
{

//  char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);

    sprintf(DXL.buftx,"G%02X", par1);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx)); 
    d = ACKLEN;
    er = DXL_RxStr(DXL.bufrx, &d);
    d = atoi(DXL.bufrx);

    return d;

}


/*   28   */
//verify the if the work position has been reaced
int AMS_TestWork(int id, int par1)
{

    char buf[80];
    int d, er; 

    //Assign Address
    AMS_assadd(id);


    sprintf(DXL.buftx,"H%02X", par1);
    DXL_TxStr(DXL.buftx, strlen(DXL.buftx)); 
    d = ACKLEN;
    er = DXL_RxStr(buf, &d);

    return er;

}



void Message(char *str, int er)
{
    char    buf[80];
    MOBJECT sh, lbl;

    sprintf(buf, "DEBUG: AMS%d, Err = %d", DXL.add+1, er);

    sh = MCreateShell(buf, SF_MODAL);
    MObjectSetBackgroundRGB(sh, DPAL.DB_BG_Plbl[0], DPAL.DB_BG_Plbl[1], DPAL.DB_BG_Plbl[2]);

    lbl = MCreateLabel(sh, str, DEFAULT_FONT);
    MObjectSetForegroundRGB(lbl, DPAL.DB_FG_Plbl[0], DPAL.DB_FG_Plbl[1], DPAL.DB_FG_Plbl[2]);
    MLabelSetAlignment(lbl, LEFT_ALIGN);

    sprintf(buf, "Txed: %s", DXL.buftx);
    lbl = MCreateLabel(sh, buf, DEFAULT_FONT);
    MObjectSetForegroundRGB(lbl, DPAL.DB_FG_Plbl[0], DPAL.DB_FG_Plbl[1], DPAL.DB_FG_Plbl[2]);
    MLabelSetAlignment(lbl, LEFT_ALIGN);

    sprintf(buf, "Rxed: %s", DXL.bufrx);
    lbl = MCreateLabel(sh, buf, DEFAULT_FONT);
    MObjectSetForegroundRGB(lbl, DPAL.DB_FG_Plbl[0], DPAL.DB_FG_Plbl[1], DPAL.DB_FG_Plbl[2]);
    MLabelSetAlignment(lbl, LEFT_ALIGN);

    MShellRealize(sh);
}


void delay(unsigned long d)
{
    DWORD   s, e;
    s = GetTickCount();
    do
        e = GetTickCount();
    while((unsigned long)labs(e - s) < d);
}

/*
void Positioning(MOBJECT p, long tout)
{
    unsigned long   st, et;
    char buf[80];
    st = GetTickCount();
    do
    {
        et = GetTickCount();
        MLoopWhileEvents(1);
        sprintf(buf, "%d sec. left", ((labs(et - st) - tout) / 1000));
        MObjectSetText(p, buf);
    }while(labs(et - st) < tout + 50);

}

*/

unsigned char DXL_ByteHex(unsigned char val)
{
    unsigned char ch = val;
    if(ch <= 9) ch += '0';
    else ch += '0' + 7;
    return ch;
}

void DXL_TxByte(unsigned char v)
{
    SioPutc(DXL.com, v);
}

void DXL_TxStr(char *tx, int len)
{
    unsigned char cksm = 0, ch;
    int     c;

    DXL_TxByte('$');

    ch = DXL_ByteHex((unsigned char) ((DXL.add >> 4) & 0x0F));
    DXL_TxByte(ch);
    cksm += ch;
    ch = DXL_ByteHex((unsigned char) (DXL.add & 0x0F));
    DXL_TxByte(ch);
    cksm += ch;
    for(c = 0; c < len; c++)
    {
        DXL_TxByte(tx[c]);
        cksm += tx[c];
    }
    DXL_TxByte('#');
    ch = DXL_ByteHex((unsigned char) ((cksm >> 4) & 0x0F));
    DXL_TxByte(ch);
    ch = DXL_ByteHex((unsigned char) (cksm & 0x0F));
    DXL_TxByte(ch);
    DXL_TxByte(13);
}

/*
 * Parameters:
 *  rx = received string
 *  *len = num of chars to be received
 * Output:
 *  *len = num of received chars
 * 
 */
int DXL_RxStr(char *rx, int *len)
{
    unsigned char cksm = 0, rcksm, add;
    int     c, p = 0;
    DWORD   st, et;

    DXL.bufrx[0] = DXL.bufrx[1] = 0;

    st = GetTickCount();
    do
    {
        c = SioRxQue(DXL.com);
        et = GetTickCount();
    //}while(c < *len && labs(et - st) < 2000);
    }while(c < *len && labs(et - st) < 500);

    SioGets(DXL.com, DXL.bufrx, c);
    
    
    while(DXL.bufrx[p++] != '$' && p < 80);
    
    if(p == 80)
        // No start character
        return 1;
    p--;
    add = DXL_HexBin(&DXL.bufrx[p + 1]);
    if(add != DXL.add)
        // Incongruent address
        return 2;
    cksm += DXL.bufrx[p + 1];
    cksm += DXL.bufrx[p + 2];
    for(c = p + 3; c < 80; c++)
    {
        if(DXL.bufrx[c] != '#')
        {
            cksm += DXL.bufrx[c];
            rx[c - 3] = DXL.bufrx[c];
        }
        else
        {
            c++;
            rcksm = DXL_HexBin(&DXL.bufrx[c]);
            rx[c - 4] = 0;
            break;
        }
    }
    if(rcksm != cksm)
        // CheckSum error
        return 3;
    *len = c - 3;
    return 0;
}



// Open serial comunications
int DXL_Open(int addr, int com, int baud)
{
    int     er = 0;
    DXL.speed = baud;
    DXL.com = com;
    DXL.add = addr; // Default device address

    SioReset(DXL.com, 64, 64);
    SioBaud(DXL.com, Baud19200);
    SioParms(DXL.com, NoParity, OneStopBit, WordLength8);


    return er;
}
// Close serial comunications
int DXL_Close(void)
{
    int er;
    er = SioReset(DXL.com, 64, 64);
    return(SioDone(DXL.com));
}