Secret_Betago
first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
BEAR_Motion
by 
BE@R lab
Diff: main.cpp
- Revision:
- 28:b3509fd32b00
- Parent:
- 27:718fc94e40ad
- Child:
- 29:5db0a9261161
--- a/main.cpp Tue Jan 26 20:37:00 2016 +0000
+++ b/main.cpp Tue Jan 26 20:47:53 2016 +0000
@@ -196,7 +196,7 @@
int i;
for(i=0; i<4; i++) {
UpMargin[0+i]=cmd[1+i];
- printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
+ //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
}
break;
}
@@ -361,11 +361,13 @@
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
memory.write(ADDRESS_HEIGHT,data_buff);
+ wait_ms(1);
} else if(cmd[1]==BODY_WIDTH) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width);
memory.write(ADDRESS_BODY_WIDTH,data_buff);
+ wait_ms(1);
} else if(cmd[1]==OFFSET) {
uint8_t y_offset_array[4];
@@ -378,7 +380,9 @@
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(1);
memory.write(ADDRESS_OFFSET+4,z_data_buffer);
+ wait_ms(1);
} else if(cmd[1]==MAG_DATA) {
uint8_t x_max_array[4];
@@ -403,38 +407,54 @@
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(1);
memory.write(ADDRESS_MAG_DATA+4,x_min_buffer);
+ wait_ms(1);
memory.write(ADDRESS_MAG_DATA+8,y_max_buffer);
+ wait_ms(1);
memory.write(ADDRESS_MAG_DATA+12,y_min_buffer);
+ wait_ms(1);
memory.write(ADDRESS_MAG_DATA+16,z_max_buffer);
+ wait_ms(1);
memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
+ wait_ms(1);
+
}
}
// else {
if (cmd[1]==ID) {
memory.write(ADDRESS_ID,id);
+ wait_ms(1);
} else if(cmd[1]==UP_MARGIN) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
memory.write(ADDRESS_UP_MARGIN,data_buff);
- printf("save OK!!\n\r");
+ wait_ms(1);
+ //printf("save OK!!\n\r");
} else if (cmd[1]==LOW_MARGIN) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin);
memory.write(ADDRESS_LOW_MARGIN,data_buff);
+ wait_ms(1);
} else if (cmd[1]==PID_UPPER_MOTOR) {
memory.write(ADDRESS_UPPER_KP,U_Kc);
+ wait_ms(1);
memory.write(ADDRESS_UPPER_KI,U_Ti);
+ wait_ms(1);
memory.write(ADDRESS_UPPER_KD,U_Td);
+ wait_ms(1);
} else if (cmd[1]==PID_LOWER_MOTOR) {
memory.write(ADDRESS_LOWER_KP,L_Kc);
+ wait_ms(1);
memory.write(ADDRESS_LOWER_KI,L_Ti);
+ wait_ms(1);
memory.write(ADDRESS_LOWER_KD,L_Td);
+ wait_ms(1);
} else if (cmd[1]==ANGLE_RANGE_UP) {
uint8_t max_array[4];
@@ -447,7 +467,9 @@
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(1);
memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
+ wait_ms(1);
} else if (cmd[1]==ANGLE_RANGE_LOW) {
uint8_t max_array[4];
@@ -460,188 +482,192 @@
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(1);
memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
+ wait_ms(1);
} else if (cmd[1]==UP_LINK_LENGTH) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength);
memory.write(ADDRESS_UP_LINK_LENGTH,data_buff);
+ wait_ms(1);
} else if (cmd[1]==LOW_LINK_LENGTH) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength);
memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff);
+ wait_ms(1);
} 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);
- uint8_t status;
- status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
- break;
- }
- case UP_MARGIN: {
- int32_t data_buff;
- com.sendUpMargin(MY_ID,UpMargin);
-
- 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;
+ wait_ms(1);
}
break;
}
+ break;
+ }
+ break;
+ }
+ case READ_DATA: {
+ switch (cmd[0]) {
+ case MOTOR_UPPER_ANG: {
+
+ com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+ uint8_t status;
+ status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+ break;
+ }
+ case UP_MARGIN: {
+ int32_t data_buff;
+ com.sendUpMargin(MY_ID,UpMargin);
+
+ 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()
{