File content as of revision 14:cf43df0ddb93:

#include "mbed.h"
#include "Receiver.h"
#include "rplidar.h"
RPLidar lidar1;

DigitalOut rs485_dirc(RS485_DIRC);

Bear_Receiver::Bear_Receiver(PinName tx,PinName rx,int baudrate)
{
    com = new COMMUNICATION(tx,rx,baudrate);
}

uint8_t Bear_Receiver::ReceiveCommand(uint8_t *id,uint8_t *data_array,uint8_t *ins)
{
    ANDANTE_PROTOCOL_PACKET package;

    rs485_dirc=0;
    wait_us(RS485_DELAY);
    uint8_t status = com->receiveCommunicatePacket(&package);

    if(status == ANDANTE_ERRBIT_NONE) {

        *id = package.robotId;
        for(int i=0; i<30; i++) {
            data_array[i] = package.parameter[i];
        }
        *ins=package.instructionErrorId;

    }
    return status;
}





void Bear_Receiver::FloatSep(float input_float,uint8_t *int_data_array,uint8_t *float_data_array)
{
    float float_buffer;
    float int_buffer;
    int16_t integer;
    int16_t floating_point;

    float_buffer=modf(input_float,&int_buffer);
    float_buffer*=FLOAT_CONVERTER;
    integer=(int16_t)int_buffer;
    floating_point=(int16_t)float_buffer;
    Utilities::ConvertInt16ToUInt8Array(integer,int_data_array);
    Utilities::ConvertInt16ToUInt8Array(floating_point,float_data_array);
}




uint8_t Bear_Receiver::sendMotorPos(uint8_t id,float up_angle,float low_angle)
{
    uint8_t IntUpAngle[2],FloatUpAngle[2];
    uint8_t IntLowAngle[2],FloatLowAngle[2];
    FloatSep(up_angle,IntUpAngle,FloatUpAngle);
    FloatSep(low_angle,IntLowAngle,FloatLowAngle);

    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 10;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=IntUpAngle[0];
    package.parameter[1]=IntUpAngle[1];
    package.parameter[2]=FloatUpAngle[0];
    package.parameter[3]=FloatUpAngle[1];
    package.parameter[4]=IntLowAngle[0];
    package.parameter[5]=IntLowAngle[1];
    package.parameter[6]=FloatLowAngle[0];
    package.parameter[7]=FloatLowAngle[1];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);


}



uint8_t Bear_Receiver::sendUpMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
{
    uint8_t IntKp[2],FloatKp[2];
    uint8_t IntKi[2],FloatKi[2];
    uint8_t IntKd[2],FloatKd[2];

    FloatSep(Kp,IntKp,FloatKp);
    FloatSep(Ki,IntKi,FloatKi);
    FloatSep(Kd,IntKd,FloatKd);


    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 14;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=IntKp[0];
    package.parameter[1]=IntKp[1];
    package.parameter[2]=FloatKp[0];
    package.parameter[3]=FloatKp[1];
    package.parameter[4]=IntKi[0];
    package.parameter[5]=IntKi[1];
    package.parameter[6]=FloatKi[0];
    package.parameter[7]=FloatKi[1];
    package.parameter[8]=IntKd[0];
    package.parameter[9]=IntKd[1];
    package.parameter[10]=FloatKd[0];
    package.parameter[11]=FloatKd[1];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}




uint8_t Bear_Receiver::sendLowMotorKpKiKd(uint8_t id,float Kp,float Ki,float Kd)
{
    uint8_t IntKp[2],FloatKp[2];
    uint8_t IntKi[2],FloatKi[2];
    uint8_t IntKd[2],FloatKd[2];

    FloatSep(Kp,IntKp,FloatKp);
    FloatSep(Ki,IntKi,FloatKi);
    FloatSep(Kd,IntKd,FloatKd);


    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 14;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=IntKp[0];
    package.parameter[1]=IntKp[1];
    package.parameter[2]=FloatKp[0];
    package.parameter[3]=FloatKp[1];
    package.parameter[4]=IntKi[0];
    package.parameter[5]=IntKi[1];
    package.parameter[6]=FloatKi[0];
    package.parameter[7]=FloatKi[1];
    package.parameter[8]=IntKd[0];
    package.parameter[9]=IntKd[1];
    package.parameter[10]=FloatKd[0];
    package.parameter[11]=FloatKd[1];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}



uint8_t Bear_Receiver::sendUpMargin(uint8_t id,float Margin)
{
    uint8_t Int[2],Float[2];
    FloatSep(Margin,Int,Float);
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Int[0];
    package.parameter[1]=Int[1];
    package.parameter[2]=Float[0];
    package.parameter[3]=Float[1];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);

}

uint8_t Bear_Receiver::sendLowMargin(uint8_t id,float Margin)
{
    uint8_t Int[2],Float[2];
    FloatSep(Margin,Int,Float);
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Int[0];
    package.parameter[1]=Int[1];
    package.parameter[2]=Float[0];
    package.parameter[3]=Float[1];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendHeight(uint8_t id,uint8_t *Height)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Height[0];
    package.parameter[1]=Height[1];
    package.parameter[2]=Height[2];
    package.parameter[3]=Height[3];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendWheelPos(uint8_t id,uint8_t *WheelPos)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=WheelPos[0];
    package.parameter[1]=WheelPos[1];
    package.parameter[2]=WheelPos[2];
    package.parameter[3]=WheelPos[3];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendMagData(uint8_t id,uint8_t *Mag)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 26;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Mag[0];
    package.parameter[1]=Mag[1];
    package.parameter[2]=Mag[2];
    package.parameter[3]=Mag[3];
    package.parameter[4]=Mag[4];
    package.parameter[5]=Mag[5];
    package.parameter[6]=Mag[6];
    package.parameter[7]=Mag[7];
    package.parameter[8]=Mag[8];
    package.parameter[9]=Mag[9];
    package.parameter[10]=Mag[10];
    package.parameter[11]=Mag[11];
    package.parameter[12]=Mag[12];
    package.parameter[13]=Mag[13];
    package.parameter[14]=Mag[14];
    package.parameter[15]=Mag[15];
    package.parameter[16]=Mag[16];
    package.parameter[17]=Mag[17];
    package.parameter[18]=Mag[18];
    package.parameter[19]=Mag[19];
    package.parameter[20]=Mag[20];
    package.parameter[21]=Mag[21];
    package.parameter[22]=Mag[22];
    package.parameter[23]=Mag[23];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendOffset(uint8_t id,uint8_t *Offset)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 10;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Offset[0];
    package.parameter[1]=Offset[1];
    package.parameter[2]=Offset[2];
    package.parameter[3]=Offset[3];
    package.parameter[4]=Offset[4];
    package.parameter[5]=Offset[5];
    package.parameter[6]=Offset[6];
    package.parameter[7]=Offset[7];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendBodyWidth(uint8_t id,uint8_t *BodyWidth)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=BodyWidth[0];
    package.parameter[1]=BodyWidth[1];
    package.parameter[2]=BodyWidth[2];
    package.parameter[3]=BodyWidth[3];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendUpAngleRange(uint8_t id,uint8_t *Angle)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 10;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Angle[0];
    package.parameter[1]=Angle[1];
    package.parameter[2]=Angle[2];
    package.parameter[3]=Angle[3];
    package.parameter[4]=Angle[4];
    package.parameter[5]=Angle[5];
    package.parameter[6]=Angle[6];
    package.parameter[7]=Angle[7];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}


uint8_t Bear_Receiver::sendLowAngleRange(uint8_t id,uint8_t *Angle)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 10;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Angle[0];
    package.parameter[1]=Angle[1];
    package.parameter[2]=Angle[2];
    package.parameter[3]=Angle[3];
    package.parameter[4]=Angle[4];
    package.parameter[5]=Angle[5];
    package.parameter[6]=Angle[6];
    package.parameter[7]=Angle[7];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);
}

uint8_t Bear_Receiver::sendUpLinkLength(uint8_t id,uint8_t *Length)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Length[0];
    package.parameter[1]=Length[1];
    package.parameter[2]=Length[2];
    package.parameter[3]=Length[3];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);

}

uint8_t Bear_Receiver::sendLowLinkLength(uint8_t id,uint8_t *Length)
{
    ANDANTE_PROTOCOL_PACKET package;

    package.robotId = id;
    package.length = 6;
    package.instructionErrorId = WRITE_DATA;
    package.parameter[0]=Length[0];
    package.parameter[1]=Length[1];
    package.parameter[2]=Length[2];
    package.parameter[3]=Length[3];

    rs485_dirc=1;
    wait_us(RS485_DELAY);
    return com->sendCommunicatePacket(&package);

}
uint8_t Bear_Receiver::sendlidar()
{
                      int i=0,j=1,k=0;
                         uint8_t intData[2]={0x00,0x01},floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 22;//122
                        package.instructionErrorId = WRITE_DATA;
                       
                        while(k<60)
                        { //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                         FloatSep( lidar1.Data[k],intData,floatData);
                         package.parameter[i]=intData[0];
                         package.parameter[j]=intData[1];
                         i=i+2;
                         j=j+2;
                         k++;
                         
                        }
                    
                       rs485_dirc=1;
                         wait_us(RS485_DELAY);
                       return com->sendCommunicatePacket(&package);

}
uint8_t Bear_Receiver::sendlidar2()
{
                     int i=0,j=1,k=60;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                        
                        while(k<120)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                 FloatSep( lidar1.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                            
                        }
                        //rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                        return   com->sendCommunicatePacket(&package);

}
uint8_t Bear_Receiver::sendlidar3()
{
                     int i=0,j=1,k=120;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<180)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                FloatSep( lidar1.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        //rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                       return   com->sendCommunicatePacket(&package);

}
uint8_t Bear_Receiver::sendlidar4()
{
                      int i=0,j=1,k=180;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<240)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                FloatSep( lidar1.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                       // rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                        return com->sendCommunicatePacket(&package);

}
uint8_t Bear_Receiver::sendlidar5()
{
                   int i=0,j=1,k=240;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<300)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                FloatSep( lidar1.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        
                       // rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         return com->sendCommunicatePacket(&package);
}
uint8_t Bear_Receiver::sendlidar6()
{
                        int i=0,j=1,k=300;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = 0x00;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<360)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                FloatSep( lidar1.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                       // rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         return com->sendCommunicatePacket(&package);
 
}