Diff: main.cpp.orig

Revision:

23:999dd41eef14

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Tue Jan 26 18:34:51 2016 +0000
@@ -0,0 +1,684 @@
+//*****************************************************/
+//  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();
+    }*/
+}
+
+
+
+
+
+