Diff: main.cpp.orig

Revision:

25:c66a0d029e1f

Parent:

23:999dd41eef14

Parent:

24:d4942687f026

Child:

26:fbccc263a588

--- a/main.cpp.orig	Tue Jan 26 18:34:51 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,684 +0,0 @@
-//*****************************************************/
-//  Include  //
-#include "mbed.h"
-#include "pinconfig.h"
-#include "PID.h"
-#include "Motor.h"
-#include "eeprom.h"
-#include "Receiver.h"
-#include "Motion_EEPROM_Address.h"
-
-//*****************************************************/
-//--PID parameter--
-//-Upper-//
-float U_Kc;
-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_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;
-int Upper_Position;
-int 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);
-//*****************************************************/
-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
-    };
-
-    while(Val != codes[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 *cmd,uint8_t ins)
-{
-    if(id==MY_ID) {
-        switch (ins) {
-            case PING: {
-                break;
-            }
-            case WRITE_DATA: {
-                switch (cmd[0]) {
-                    case ID: {
-                        ///
-                        MY_ID = (int16_t)cmd[1];
-                        break;
-                    }
-                    case MOTOR_UPPER_ANG: {
-                        uint8_t IntUpAngle[2],FloatUpAngle[2];
-                        uint8_t IntLowAngle[2],FloatLowAngle[2];
-                        float int_buffer,float_buffer;
-
-                        IntUpAngle[0]=cmd[1];
-                        IntUpAngle[1]=cmd[2];
-                        FloatUpAngle[0]=cmd[3];
-                        FloatUpAngle[1]=cmd[4];
-                        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
-                        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
-                        up_angle=int_buffer+float_buffer;
-
-                        IntLowAngle[0]=cmd[5];
-                        IntLowAngle[1]=cmd[6];
-                        FloatLowAngle[0]=cmd[7];
-                        FloatLowAngle[1]=cmd[8];
-                        int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
-                        float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
-                        low_angle=int_buffer+float_buffer;
-                        break;
-                    }
-                    case UP_MARGIN: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            UpMargin[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case LOW_MARGIN: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            LowMargin[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case KP_UPPER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        U_Kc=Int+Float;
-                        break;
-                    }
-                    case KI_UPPER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float,KI;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        KI=Int+Float;
-                        U_Ti=KI/U_Kc;
-                        break;
-                    }
-                    case KD_UPPER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float,KD;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        KD=Int+Float;
-                        U_Td=KD/U_Kc;
-                        break;
-                    }
-                    case KP_LOWER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        L_Kc=Int+Float;
-                        break;
-                    }
-                    case KI_LOWER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float,KI;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        KI=Int+Float;
-                        L_Ti=KI/L_Kc;
-                        break;
-                    }
-                    case KD_LOWER_MOTOR: {
-                        uint8_t int_buffer[2];
-                        uint8_t float_buffer[2];
-                        float Int,Float,KD;
-                        int_buffer[0]=cmd[1];
-                        int_buffer[1]=cmd[2];
-                        float_buffer[0]=cmd[3];
-                        float_buffer[1]=cmd[4];
-                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
-                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
-                        KD=Int+Float;
-                        L_Td=KD/L_Kc;
-                        break;
-                    }
-                    case HEIGHT: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            Height[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case WHEELPOS: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            Wheelpos[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case MAG_DATA: {
-                        int i;
-                        for(i=0; i<24; i++) {
-                            Mag[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case OFFSET: {
-                        int i;
-                        for(i=0; i<8; i++) {
-                            Offset[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case BODY_WIDTH: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            Body_width[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case ANGLE_RANGE_UP: {
-                        int i;
-                        for(i=0; i<8; i++) {
-                            Angle_Range_Up[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case ANGLE_RANGE_LOW: {
-                        int i;
-                        for(i=0; i<8; i++) {
-                            Angle_Range_Low[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-
-                    case UP_LINK_LENGTH: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            UpLinkLength[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    case LOW_LINK_LENGTH: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            LowLinkLength[0+i]=cmd[1+i];
-                        }
-                        break;
-                    }
-                    // unfinish yet!!!!!!!!!!!!!!!!!
-                    case SAVE_EEPROM_DATA: {
-                        if(id==0x01) {
-
-                            if (cmd[1]==HEIGHT) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
-                                memory.write(ADDRESS_HEIGHT,data_buff);
-
-                            } else if(cmd[1]==BODY_WIDTH) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width);
-                                memory.write(ADDRESS_BODY_WIDTH,data_buff);
-
-                            } else if(cmd[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);
-                                memory.write(ADDRESS_OFFSET+4,z_data_buffer);
-
-                            } else if(cmd[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);
-                                memory.write(ADDRESS_MAG_DATA+4,x_min_buffer);
-                                memory.write(ADDRESS_MAG_DATA+8,y_max_buffer);
-                                memory.write(ADDRESS_MAG_DATA+12,y_min_buffer);
-                                memory.write(ADDRESS_MAG_DATA+16,z_max_buffer);
-                                memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
-                            }
-
-                        } else {
-                            if (cmd[1]==ID) {
-                                memory.write(ADDRESS_ID,id);
-
-                            } else if(cmd[1]==UP_MARGIN) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
-                                memory.write(ADDRESS_UP_MARGIN,data_buff);
-
-                            } else if (cmd[1]==LOW_MARGIN) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin);
-                                memory.write(ADDRESS_LOW_MARGIN,data_buff);
-
-                            } else if (cmd[1]==PID_UPPER_MOTOR) {
-                                memory.write(ADDRESS_UPPER_KP,U_Kc);
-                                memory.write(ADDRESS_UPPER_KI,U_Ti);
-                                memory.write(ADDRESS_UPPER_KD,U_Td);
-
-                            } else if (cmd[1]==PID_LOWER_MOTOR) {
-                                memory.write(ADDRESS_LOWER_KP,L_Kc);
-                                memory.write(ADDRESS_LOWER_KI,L_Ti);
-                                memory.write(ADDRESS_LOWER_KD,L_Td);
-
-                            } else if (cmd[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);
-                                memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
-
-                            } else if (cmd[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);
-                                memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
-
-                            } else if (cmd[1]==UP_LINK_LENGTH) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength);
-                                memory.write(ADDRESS_UP_LINK_LENGTH,data_buff);
-
-                            } else if (cmd[1]==LOW_LINK_LENGTH) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength);
-                                memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff);
-
-                            } else if (cmd[1]==WHEELPOS) {
-                                int32_t data_buff;
-                                data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos);
-                                memory.write(ADDRESS_WHEELPOS,data_buff);
-
-                            }
-                        }
-                        break;
-                    }
-
-                    //   break;
-                }
-                case READ_DATA: {
-                    switch (cmd[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);
-                            memory.read(ADDRESS_UPPER_KD,U_Kd);
-                            com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd);
-                            break;
-                        }
-                        case PID_LOWER_MOTOR: {
-                            memory.read(ADDRESS_LOWER_KP,L_Kc);
-                            memory.read(ADDRESS_LOWER_KI,L_Ki);
-                            memory.read(ADDRESS_LOWER_KD,L_Kd);
-                            com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
-                            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()
-{
-    uint8_t data_array[30];
-    uint8_t id;
-    uint8_t ins;
-    uint8_t status;
-
-    status = com.ReceiveCommand(&id,data_array,&ins);
-    if(status == ANDANTE_ERRBIT_NONE) {
-        CmdCheck((int16_t)id,data_array,ins);
-    }
-}
-/*******************************************************/
-int main()
-{
-    printf("Before Read : %d,%d,%d and %d\r\n",Offset[0],Offset[1],Offset[2],Offset[3]);
-    
-    /*Recieve.attach(&Rc,0.000001);
-    Start_Up();
-    SET_UpperPID();
-    SET_LowerPID();
-
-    while(1) {
-        //Set Set_point
-        Up_PID.setSetPoint(Upper_SetPoint);
-        Low_PID.setSetPoint(Lower_SetPoint);
-
-        //Control Motor
-        Move_Upper();
-        Move_Lower();
-    }*/
-}
-
-
-
-
-
-