first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
main.cpp
- Committer:
- icyzkungz
- Date:
- 2016-06-07
- Revision:
- 37:8719223998d8
- Parent:
- 36:1561b6d61095
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" #include "pidcontrol.h" #define EEPROM_DELAY 2 //#define DEBUG_UP //#define DEBUG_LOW //*****************************************************/ //--PID parameter-- //-Upper-// float U_Kc=0.2; float U_Ki_gain=0.0; float U_Kd_gain=0.0; float U_Ti=0.0; float U_Td=0.0; float U_Ki=U_Kc*U_Ti; float U_Kd=U_Kc*U_Td; //-lower-// float L_Kc=0.2; float L_Ki_gain=0.0; float L_Kd_gain=0.0; float L_Ti=0.0; float L_Td=0.0; 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); float UpMargin; float LowMargin; 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=20; int Lower_SetPoint=8; //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); PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate PID Low_PID(L_Kc, L_Ti, L_Td); //*****************************************************/ void Start_Up(); void CmdCheck(int16_t id,uint8_t *command,uint8_t ins); Timer counterUP; Timer counterLOW; 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); memory.read(ADDRESS_UP_MARGIN,UpMargin); Up_PID.setMargin(UpMargin); memory.read(ADDRESS_UPPER_KP,U_Kc); Up_PID.setKp(U_Kc); memory.read(ADDRESS_UPPER_KI,U_Ki_gain); Up_PID.setKi(U_Ki_gain); memory.read(ADDRESS_UPPER_KD,U_Kd_gain); Up_PID.setKd(U_Kd_gain); //Up_PID.setMode(0); //Up_PID.setInputLimits(18,62); //Up_PID.setOutputLimits(0,1); } //******************************************************/ void SET_LowerPID() { Lower.period(0.001); memory.read(ADDRESS_LOW_MARGIN,LowMargin); Low_PID.setMargin(LowMargin); memory.read(ADDRESS_LOWER_KP,L_Kc); Low_PID.setKp(L_Kc); memory.read(ADDRESS_LOWER_KI,L_Ki_gain); Low_PID.setKi(L_Ki_gain); memory.read(ADDRESS_LOWER_KD,L_Kd_gain); Low_PID.setKd(L_Kd_gain); //Low_PID.setMode(0); //Low_PID.setInputLimits(0,127); // set range //Low_PID.setOutputLimits(0,1); } //******************************************************/ void Move_Upper() { Read_Encoder(EncoderA); Upper_Position = (float)Get_EnValue(data); #ifdef DEBUG_UP printf("read_encode = 0x%2x \n\r",data); printf("Setpoint = %d\n\r",Upper_SetPoint); printf("Upper_Position = %f\n\r",Upper_Position); #endif Up_PID.setCurrent(Upper_Position); float speed =Up_PID.compute(); #ifdef DEBUG_UP printf("E_n= %f\n\r",Up_PID.getErrorNow()); printf("Kp = %f\n\r",Up_PID.getKp()); printf("speed = %f\n\n\n\r",speed); #endif Upper.speed(speed); } //******************************************************/ void Move_Lower() { Read_Encoder(EncoderB); Lower_Position = (float)Get_EnValue(data); Low_PID.setCurrent(Lower_Position); #ifdef DEBUG_LOW printf("read_encode = 0x%2x \n\r",data); printf("Setpoint = %d\n\r",Lower_SetPoint); printf("Upper_Position = %f\n\r",Lower_Position); #endif float speed =Low_PID.compute(); #ifdef DEBUG_LOW printf("E_n= %f\n\r",Low_PID.getErrorNow()); printf("Kp = %f\n\r",Low_PID.getKp()); printf("speed = %f\n\n\n\r",speed); #endif Lower.speed(speed); } //******************************************************/ 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 ID:0x%02x\n\r",MY_ID); /* while(1) { Upper.speed(-1); wait_ms(400); Upper.speed(1); wait_ms(400); Upper.break(); Lower.speed(-1.0); wait_ms(401); Lower.speed(1.0); wait_ms(401); Lower.break(); } */ // counterUP.start(); // counterLOW.start(); while(1) { /* //printf("timer = %d\n\r",counter.read_ms()); if(counterUP.read_ms() > 1400) { if(Upper_SetPoint < 53) { Upper_SetPoint++; } else Upper_SetPoint =18; counterUP.reset(); } if(counterLOW.read_ms() > 700) { if(Lower_SetPoint < 100) { Lower_SetPoint++; } else Lower_SetPoint =8; counterLOW.reset(); } */ myled =0; //wait_ms(10); ///////////////////////////////////////////////////// start //Set Set_point Up_PID.setGoal(Upper_SetPoint); Low_PID.setGoal(Lower_SetPoint); //Control Motor Move_Upper(); Move_Lower(); ///////////////////////////////////////////////////// stop =306us //uint8_t aaa[1]={10}; //com.sendBodyWidth(0x01,aaa); Rc(); //wait_ms(1); } } 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_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); //printf("Up Angle = %f\n",up_angle); IntLowAngle[0]=command[5]; IntLowAngle[1]=command[6]; Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); //printf("Low Angle = %f\n",low_angle); break; } case UP_MARGIN: { 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; UpMargin=Int+Float; Up_PID.setMargin(UpMargin); //printf("Kp Upper : %f\r\n",U_Kc); break; } case LOW_MARGIN: { 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; LowMargin=Int+Float; Low_PID.setMargin(LowMargin); //printf("Kp Upper : %f\r\n",U_Kc); 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); Up_PID.setKp(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; Up_PID.setKi(U_Ki_gain); //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; Up_PID.setKd(U_Kd_gain); //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) { memory.write(ADDRESS_UP_MARGIN,UpMargin); wait_ms(EEPROM_DELAY); //printf("save OK!!\n\r"); } else if (command[1]==LOW_MARGIN) { memory.write(ADDRESS_LOW_MARGIN,LowMargin); 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: { memory.read(ADDRESS_UP_MARGIN,UpMargin); com.sendUpMargin(MY_ID,UpMargin); break; } case LOW_MARGIN: { memory.read(ADDRESS_LOW_MARGIN,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; } } } } }