Betago / Communication_Robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of Communication_Robot by palm and chin

Receiver.cpp

Committer:
palmdotax
Date:
2016-03-21
Revision:
11:3c11a0355a3e
Child:
13:bc19774be4df

File content as of revision 11:3c11a0355a3e:

#include "mbed.h"
#include "Receiver.h"


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);

}