File content as of revision 37:8719223998d8:

//*****************************************************/
//  Include  //
#include "mbed.h"
#include "pinconfig.h"
#include "PID.h"
#include "Motor.h"
#include "eeprom.h"
#include "Receiver.h"
#include "Motion_EEPROM_Address.h"
#define EEPROM_DELAY 2
//*****************************************************/
//--PID parameter--
//-Upper-//
float U_Kc;
float U_Ki_gain;
float U_Kd_gain;
float U_Ti;
float U_Td;
float U_Ki=U_Kc*U_Ti;
float U_Kd=U_Kc*U_Td;
//-lower-//
float L_Kc;
float L_Ki_gain;
float L_Kd_gain;
float L_Ti;
float L_Td;
float L_Ki=L_Kc*L_Ti;
float L_Kd=L_Kc*L_Td;
//*****************************************************/
// Global  //
Ticker Recieve;
//-- Communication --
Serial PC(D1,D0);
Bear_Receiver com(Tx,Rx,1000000);
int16_t MY_ID = 0x01;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
uint8_t UpMargin[4];
uint8_t LowMargin[4];
uint8_t Height[4];
uint8_t Wheelpos[4];
uint8_t Mag[24];
uint8_t Offset[8];//={1,2,3,4,5,6,7,8};
uint8_t Body_width[4];
uint8_t Angle_Range_Up[8];
uint8_t Angle_Range_Low[8];
uint8_t UpLinkLength[4];
uint8_t LowLinkLength[4];
//-- encoder --
float up_angle,low_angle;
float Upper_Position;
float Lower_Position;
int data;
SPI ENC(Emosi, Emiso, Esck);
DigitalOut EncA(EncoderA);
DigitalOut EncB(EncoderB);
//-- Motor --
int dir;
Motor Upper(PWM_LU,A_LU,B_LU);
Motor Lower(PWM_LL,A_LL,B_LL);
//-- PID --
int Upper_SetPoint;
int Lower_SetPoint;
PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
//*****************************************************/

DigitalOut myled(LED1);


void Read_Encoder(PinName Encoder)
{
    ENC.format(8,0);
    ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k

    if(Encoder == EncoderA) {
        EncA = 0;
    } else {
        EncB = 0;
    }
    ENC.write(0x41);
    ENC.write(0x09);
    data = ENC.write(0x00);
    if(Encoder == EncoderA) {
        EncA = 1;
    } else {
        EncB = 1;
    }

}
//*****************************************************/
int Get_EnValue(int Val)
{
    int i = 0;
    static unsigned char codes[] = {
        127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
        191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
        223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
        239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
        247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
        251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
        253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
        254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
    };
    if ( MY_ID == 0x01 )//when it was left side
    {
        while(Val != codes[i]) {
            i++;
            }
    }
    if ( MY_ID == 0x02 )//when it was right side
    {
        while(Val != codes[127-i]) {
            i++;
            }
    }
    return i;

}
//*****************************************************/
void SET_UpperPID()
{
    Upper.period(0.001);
    Up_PID.setMode(0);
    Up_PID.setInputLimits(0,127);
    Up_PID.setOutputLimits(0,1);
}
//******************************************************/
void SET_LowerPID()
{
    Lower.period(0.001);
    Low_PID.setMode(0);
    Low_PID.setInputLimits(0,127); // set range
    Low_PID.setOutputLimits(0,1);
}
//******************************************************/
void Move_Upper()
{
    Read_Encoder(EncoderA);
    Upper_Position = Get_EnValue(data);

    Up_PID.setProcessValue(Upper_Position);

    if(Upper_Position - Upper_SetPoint > 0 ) {
        dir = 1;
    }
    if(Upper_Position - Upper_SetPoint < 0 ) {
        dir = -1;
    }
    Upper.speed(Up_PID.compute() * dir);
}
//******************************************************/
void Move_Lower()
{
    Read_Encoder(EncoderB);
    Lower_Position = Get_EnValue(data);

    Low_PID.setProcessValue(Lower_Position);

    if(Lower_Position - Lower_SetPoint > 0 ) {
        dir = 1;
    }
    if(Lower_Position - Lower_SetPoint < 0 ) {
        dir = -1;
    }
    Lower.speed(Low_PID.compute() * dir);
}
//******************************************************/

void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
    if(id==MY_ID) {
        switch (ins) {
            case PING: {
                break;
            }
            case WRITE_DATA: {
                switch (command[0]) {
                    case ID: {
                        ///
                        MY_ID = (int16_t)command[1];
                        break;
                    }
                    case MOTOR_UPPER_ANG: {
                        uint8_t IntUpAngle[2];
                        uint8_t IntLowAngle[2];

                        IntUpAngle[0]=command[1];
                        IntUpAngle[1]=command[2];
                        Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
                        //printf("Up Angle = %f\n",up_angle);
                        IntLowAngle[0]=command[5];
                        IntLowAngle[1]=command[6];
                        Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
                        //printf("Low Angle = %f\n",low_angle);
                        break;
                    }
                    case UP_MARGIN: {
                        int i;
                        for(i=0; i<4; i++) {
                            UpMargin[i]=command[1+i];
                            //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
                        }
                        break;
                    }
                    case LOW_MARGIN: {
                        int i;
                        for(i=0; i<4; i++) {
                            LowMargin[i]=command[1+i];
                        }
                        break;
                    }
                    case KP_UPPER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        U_Kc=Int+Float;
                        //printf("Kp Upper : %f\r\n",U_Kc);
                        break;
                    }
                    case KI_UPPER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        U_Ki_gain=Int+Float;
                        U_Ti=U_Ki_gain/U_Kc;
                        //printf("Ki Upper : %f\r\n",U_Ki_gain);
                        break;
                    }
                    case KD_UPPER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        U_Kd_gain=Int+Float;
                        U_Td=U_Kd_gain/U_Kc;
                        //printf("Kd Upper : %f\r\n",U_Kd_gain);
                        break;
                    }
                    case KP_LOWER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        L_Kc=Int+Float;
                        //printf("Kp Lower : %f\r\n",L_Kc);
                        break;
                    }
                    case KI_LOWER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        L_Ki_gain=Int+Float;
                        L_Ti=L_Ki_gain/L_Kc;
                        //printf("Ki Lower : %f\r\n",L_Ki_gain);
                        break;
                    }
                    case KD_LOWER_MOTOR: {
                        uint8_t int_buffer[2];
                        uint8_t float_buffer[2];
                        float Int,Float;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                        L_Kd_gain=Int+Float;
                        L_Td=L_Kd_gain/L_Kc;
                        //printf("Kd Lower : %f\r\n",L_Kd_gain);
                        break;
                    }
                    case HEIGHT: {
                        int i;
                        for(i=0; i<4; i++) {
                            Height[0+i]=command[1+i];
                        }
                        break;
                    }
                    case WHEELPOS: {
                        int i;
                        for(i=0; i<4; i++) {
                            Wheelpos[0+i]=command[1+i];
                        }
                        break;
                    }
                    case MAG_DATA: {
                        int i;
                        for(i=0; i<24; i++) {
                            Mag[0+i]=command[1+i];
                        }
                        break;
                    }
                    case OFFSET: {
                        int i;
                        for(i=0; i<8; i++) {
                            Offset[0+i]=command[1+i];
                        }
                        break;
                    }
                    case BODY_WIDTH: {
                        int i;
                        for(i=0; i<4; i++) {
                            Body_width[0+i]=command[1+i];
                        }
                        break;
                    }
                    case ANGLE_RANGE_UP: {
                        int i;
                        for(i=0; i<8; i++) {
                            Angle_Range_Up[i]=command[1+i];
                            //printf("%d Angle = 0x%02x\r\n",i,Angle_Range_Up[i]);
                        }
                        break;
                    }
                    case ANGLE_RANGE_LOW: {
                        int i;
                        for(i=0; i<8; i++) {
                            Angle_Range_Low[0+i]=command[1+i];
                        }
                        break;
                    }

                    case UP_LINK_LENGTH: {
                        int i;
                        for(i=0; i<4; i++) {
                            UpLinkLength[i]=command[1+i];
                        }
                        break;
                    }
                    case LOW_LINK_LENGTH: {
                        int i;
                        for(i=0; i<4; i++) {
                            LowLinkLength[i]=command[1+i];
                        }
                        break;
                    }
                    // unfinish yet!!!!!!!!!!!!!!!!!
                    case SAVE_EEPROM_DATA: {
                        if(id==0x01) {

                            if (command[1]==HEIGHT) {
                                int32_t data_buff;
                                data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
                                memory.write(ADDRESS_HEIGHT,data_buff);
                                wait_ms(EEPROM_DELAY);

                            } else if(command[1]==BODY_WIDTH) {
                                int32_t data_buff;
                                data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width);
                                memory.write(ADDRESS_BODY_WIDTH,data_buff);
                                wait_ms(EEPROM_DELAY);

                            } else if(command[1]==OFFSET) {
                                uint8_t y_offset_array[4];
                                uint8_t z_offset_array[4];
                                int32_t y_data_buffer,z_data_buffer;
                                for(int i=0; i<4; i++) {
                                    y_offset_array[i]=Offset[i];
                                    z_offset_array[i]=Offset[i+4];
                                }
                                y_data_buffer = Utilities::ConvertUInt8ArrayToInt32(y_offset_array);
                                z_data_buffer = Utilities::ConvertUInt8ArrayToInt32(z_offset_array);
                                memory.write(ADDRESS_OFFSET,y_data_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_OFFSET+4,z_data_buffer);
                                wait_ms(EEPROM_DELAY);

                            } else if(command[1]==MAG_DATA) {
                                uint8_t x_max_array[4];
                                uint8_t x_min_array[4];
                                uint8_t y_max_array[4];
                                uint8_t y_min_array[4];
                                uint8_t z_max_array[4];
                                uint8_t z_min_array[4];
                                int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer;
                                for(int i=0; i<4; i++) {
                                    x_max_array[i]=Mag[i];
                                    x_min_array[i]=Mag[i+4];
                                    y_max_array[i]=Mag[i+8];
                                    y_min_array[i]=Mag[i+12];
                                    z_max_array[i]=Mag[i+16];
                                    z_min_array[i]=Mag[i+20];
                                }
                                x_max_buffer = Utilities::ConvertUInt8ArrayToInt32(x_max_array);
                                x_min_buffer = Utilities::ConvertUInt8ArrayToInt32(x_min_array);
                                y_max_buffer = Utilities::ConvertUInt8ArrayToInt32(y_max_array);
                                y_min_buffer = Utilities::ConvertUInt8ArrayToInt32(y_min_array);
                                z_max_buffer = Utilities::ConvertUInt8ArrayToInt32(z_max_array);
                                z_min_buffer = Utilities::ConvertUInt8ArrayToInt32(z_min_array);
                                memory.write(ADDRESS_MAG_DATA,x_max_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_MAG_DATA+4,x_min_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_MAG_DATA+8,y_max_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_MAG_DATA+12,y_min_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_MAG_DATA+16,z_max_buffer);
                                wait_ms(EEPROM_DELAY);
                                memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
                                wait_ms(EEPROM_DELAY);

                            }

                        }
                        // else {
                        if (command[1]==ID) {
                            memory.write(ADDRESS_ID,id);
                            wait_ms(EEPROM_DELAY);

                        } else if(command[1]==UP_MARGIN) {
                            int32_t data_buff;
                            data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
                            memory.write(ADDRESS_UP_MARGIN,data_buff);
                            wait_ms(EEPROM_DELAY);
                            //printf("save OK!!\n\r");

                        } else if (command[1]==LOW_MARGIN) {
                            int32_t data_buff;
                            data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin);
                            memory.write(ADDRESS_LOW_MARGIN,data_buff);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==PID_UPPER_MOTOR) {
                            memory.write(ADDRESS_UPPER_KP,U_Kc);
                            //printf("U_Write : %f\r\n",U_Kc);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_UPPER_KI,U_Ki_gain);
                            //printf("U_Write : %f\r\n",U_Ki_gain);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_UPPER_KD,U_Kd_gain);
                            //printf("U_Write : %f\r\n",U_Kd_gain);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==PID_LOWER_MOTOR) {
                            memory.write(ADDRESS_LOWER_KP,L_Kc);
                            //printf("L_Write : %f\r\n",L_Kc);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_LOWER_KI,L_Ki_gain);
                            //printf("L_Write : %f\r\n",L_Ki_gain);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_LOWER_KD,L_Kd_gain);
                            //printf("L_Write : %f\r\n",L_Kd_gain);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==ANGLE_RANGE_UP) {
                            uint8_t max_array[4];
                            uint8_t min_array[4];
                            int32_t max_data_buffer,min_data_buffer;
                            for(int i=0; i<4; i++) {
                                max_array[i]=Angle_Range_Up[i];
                                min_array[i]=Angle_Range_Up[i+4];
                            }
                            max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array);
                            min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array);
                            memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==ANGLE_RANGE_LOW) {
                            uint8_t max_array[4];
                            uint8_t min_array[4];
                            int32_t max_data_buffer,min_data_buffer;
                            for(int i=0; i<4; i++) {
                                max_array[i]=Angle_Range_Low[i];
                                min_array[i]=Angle_Range_Low[i+4];
                            }
                            max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array);
                            min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array);
                            memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer);
                            wait_ms(EEPROM_DELAY);
                            memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==UP_LINK_LENGTH) {
                            int32_t data_buff;
                            data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength);
                            memory.write(ADDRESS_UP_LINK_LENGTH,data_buff);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==LOW_LINK_LENGTH) {
                            int32_t data_buff;
                            data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength);
                            memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff);
                            wait_ms(EEPROM_DELAY);

                        } else if (command[1]==WHEELPOS) {
                            int32_t data_buff;
                            data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos);
                            memory.write(ADDRESS_WHEELPOS,data_buff);
                            wait_ms(EEPROM_DELAY);
                        }
                        break;
                    }
                    break;
                }
                break;
            }
            case READ_DATA: {
                switch (command[0]) {
                    case MOTOR_UPPER_ANG: {
                        com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
                        break;
                    }
                    case UP_MARGIN: {
                        int32_t data_buff;
                        memory.read(ADDRESS_UP_MARGIN,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin);
                        com.sendUpMargin(MY_ID,UpMargin);
                        break;
                    }
                    case LOW_MARGIN: {
                        int32_t data_buff;
                        memory.read(ADDRESS_LOW_MARGIN,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,LowMargin);
                        com.sendLowMargin(MY_ID,LowMargin);
                        break;
                    }
                    case PID_UPPER_MOTOR: {
                        memory.read(ADDRESS_UPPER_KP,U_Kc);
                        memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
                        memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
                        com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki_gain,U_Kd_gain);
                        /*
                        printf("After read Kp : %f\r\n",U_Kc);
                        printf("After read Ki : %f\r\n",U_Ki_gain);
                        printf("After read Kd : %f\r\n",U_Kd_gain);
                        */
                        break;
                    }
                    case PID_LOWER_MOTOR: {
                        memory.read(ADDRESS_LOWER_KP,L_Kc);
                        memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
                        memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
                        com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki_gain,L_Kd_gain);
                        /*
                        printf("After read L_Kp : %f\r\n",L_Kc);
                        printf("After read L_Ki : %f\r\n",L_Ki_gain);
                        printf("After read L_Kd : %f\r\n",L_Kd_gain);
                        */
                        break;
                    }
                    case HEIGHT: {
                        int32_t data_buff;
                        memory.read(ADDRESS_HEIGHT,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,Height);
                        com.sendHeight(MY_ID,Height);
                        break;
                    }
                    case WHEELPOS: {
                        int32_t data_buff;
                        memory.read(ADDRESS_WHEELPOS,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,Wheelpos);
                        com.sendWheelPos(MY_ID,Wheelpos);
                        break;
                    }
                    case MAG_DATA: {
                        uint8_t x_max_array[4];
                        uint8_t x_min_array[4];
                        uint8_t y_max_array[4];
                        uint8_t y_min_array[4];
                        uint8_t z_max_array[4];
                        uint8_t z_min_array[4];
                        int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer;
                        memory.read(ADDRESS_MAG_DATA,x_max_buffer);
                        memory.read(ADDRESS_MAG_DATA+4,x_min_buffer);
                        memory.read(ADDRESS_MAG_DATA+8,y_max_buffer);
                        memory.read(ADDRESS_MAG_DATA+12,y_min_buffer);
                        memory.read(ADDRESS_MAG_DATA+16,z_max_buffer);
                        memory.read(ADDRESS_MAG_DATA+20,z_min_buffer);
                        Utilities::ConvertInt32ToUInt8Array(x_max_buffer,x_max_array);
                        Utilities::ConvertInt32ToUInt8Array(x_min_buffer,x_min_array);
                        Utilities::ConvertInt32ToUInt8Array(y_max_buffer,y_max_array);
                        Utilities::ConvertInt32ToUInt8Array(y_min_buffer,y_min_array);
                        Utilities::ConvertInt32ToUInt8Array(z_max_buffer,z_max_array);
                        Utilities::ConvertInt32ToUInt8Array(z_min_buffer,z_min_array);
                        for(int i=0; i<4; i++) {
                            Mag[i]=x_max_array[i];
                            Mag[i+4]=x_min_array[i];
                            Mag[i+8]=y_max_array[i];
                            Mag[i+12]=y_min_array[i];
                            Mag[i+16]=z_max_array[i];
                            Mag[i+20]=z_min_array[i];
                        }
                        com.sendMagData(MY_ID,Mag);
                        break;
                    }
                    case OFFSET: {
                        uint8_t y_offset_array[4];
                        uint8_t z_offset_array[4];
                        int32_t y_data_buffer,z_data_buffer;
                        memory.read(ADDRESS_OFFSET,y_data_buffer);
                        memory.read(ADDRESS_OFFSET+4,z_data_buffer);
                        Utilities::ConvertInt32ToUInt8Array(y_data_buffer,y_offset_array);
                        Utilities::ConvertInt32ToUInt8Array(z_data_buffer,z_offset_array);
                        for(int i=0; i<4; i++) {
                            Offset[i]=y_offset_array[i];
                            Offset[i+4]=z_offset_array[i];
                        }
                        com.sendOffset(MY_ID,Offset);
                        break;
                    }
                    case BODY_WIDTH: {
                        int32_t data_buff;
                        memory.read(ADDRESS_BODY_WIDTH,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,Body_width);
                        com.sendBodyWidth(MY_ID,Body_width);
                        break;
                    }
                    case ANGLE_RANGE_UP: {
                        uint8_t max_array[4];
                        uint8_t min_array[4];
                        int32_t max_data_buffer,min_data_buffer;
                        memory.read(ADDRESS_ANGLE_RANGE_UP,max_data_buffer);
                        memory.read(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
                        Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array);
                        Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array);
                        for(int i=0; i<4; i++) {
                            Angle_Range_Up[i]=max_array[i];
                            Angle_Range_Up[i+4]=min_array[i];
                        }
                        com.sendUpAngleRange(MY_ID,Angle_Range_Up);
                        break;
                    }
                    case ANGLE_RANGE_LOW: {
                        uint8_t max_array[4];
                        uint8_t min_array[4];
                        int32_t max_data_buffer,min_data_buffer;
                        memory.read(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer);
                        memory.read(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
                        Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array);
                        Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array);
                        for(int i=0; i<4; i++) {
                            Angle_Range_Low[i]=max_array[i];
                            Angle_Range_Low[i+4]=min_array[i];
                        }
                        com.sendLowAngleRange(MY_ID,Angle_Range_Low);
                        break;
                    }
                    case UP_LINK_LENGTH: {
                        int32_t data_buff;
                        memory.read(ADDRESS_UP_LINK_LENGTH,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,UpLinkLength);
                        com.sendUpLinkLength(MY_ID,UpLinkLength);
                        break;
                    }
                    case LOW_LINK_LENGTH: {
                        int32_t data_buff;
                        memory.read(ADDRESS_LOW_LINK_LENGTH,data_buff);
                        Utilities::ConvertInt32ToUInt8Array(data_buff,LowLinkLength);
                        com.sendLowLinkLength(MY_ID,LowLinkLength);
                        break;
                    }
                    break;
                }
            }
        }
    }
}


/******************************************************/
void Start_Up()
{
    // wait for reciever
    memory.read(ADDRESS_ID,MY_ID);
    memory.read(ADDRESS_UPPER_KP,U_Kc);
    memory.read(ADDRESS_UPPER_KI,U_Ti);
    memory.read(ADDRESS_UPPER_KD,U_Td);
    memory.read(ADDRESS_LOWER_KP,L_Kc);
    memory.read(ADDRESS_LOWER_KI,L_Ti);
    memory.read(ADDRESS_LOWER_KD,L_Td);

}
/*******************************************************/
void Rc()
{
    myled =1;
    uint8_t data_array[30];
    uint8_t id=0;
    uint8_t ins=0;
    uint8_t status=0xFF;



    status = com.ReceiveCommand(&id,data_array,&ins);
    //PC.printf("status = 0x%02x\n\r",status);
    if(status == ANDANTE_ERRBIT_NONE) {
        CmdCheck((int16_t)id,data_array,ins);
    }

}
/*******************************************************/
int main()
{
    PC.baud(115200);
    /*
    while(1)
    {
    Read_Encoder(EncoderA);
    Upper_Position = Get_EnValue(data);
    Read_Encoder(EncoderB);
    Lower_Position = Get_EnValue(data);
    PC.printf("Upper Position : %f\n",Upper_Position);
    PC.printf("Lower_Position : %f\n",Lower_Position);
    wait(0.5);
    }
    */


    //Recieve.attach(&Rc,0.025);
    //Start_Up();

    SET_UpperPID();
    SET_LowerPID();

    printf("BEAR MOTION\n\r");
    while(1) {
        myled =0;
        //wait_ms(10);
///////////////////////////////////////////////////// start
        //Set Set_point
        Up_PID.setSetPoint(Upper_SetPoint);
        Low_PID.setSetPoint(Lower_SetPoint);

        Read_Encoder(EncoderB);
        Lower_Position = Get_EnValue(data);
        PC.printf("position = %2f\n",Lower_Position);
        //Control Motor
        //Move_Upper();
        //Move_Lower();
/////////////////////////////////////////////////////  stop =306us
        //uint8_t aaa[1]={10};
        //com.sendBodyWidth(0x01,aaa);
        Rc();
        //wait_ms(1);
    }
}