BE@R lab
โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
DogPID
by 
Digital B14
Diff: main.cpp
- Revision:
- 27:718fc94e40ad
- Parent:
- 26:fbccc263a588
- Child:
- 28:b3509fd32b00
--- a/main.cpp Tue Jan 26 18:45:22 2016 +0000
+++ b/main.cpp Tue Jan 26 20:37:00 2016 +0000
@@ -196,6 +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]);
}
break;
}
@@ -409,77 +410,79 @@
memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
}
- } else {
- if (cmd[1]==ID) {
- memory.write(ADDRESS_ID,id);
+ }
+ // 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]==UP_MARGIN) {
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
+ memory.write(ADDRESS_UP_MARGIN,data_buff);
+ 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);
+ } 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_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]==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_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]==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]==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]==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);
+ } else if (cmd[1]==WHEELPOS) {
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos);
+ memory.write(ADDRESS_WHEELPOS,data_buff);
- }
}
+ // }
break;
}
- // break;
+ break;
}
case READ_DATA: {
switch (cmd[0]) {
@@ -492,6 +495,8 @@
}
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);
@@ -629,7 +634,7 @@
com.sendLowLinkLength(MY_ID,LowLinkLength);
break;
}
- // break;
+ break;
}
}
}
@@ -651,27 +656,27 @@
}
/*******************************************************/
-inline void Rc()
+void Rc()
{
myled =1;
uint8_t data_array[30];
- uint8_t id;
- uint8_t ins;
- uint8_t status;
+ uint8_t id=0;
+ uint8_t ins=0;
+ uint8_t status=0xFF;
-
+
status = com.ReceiveCommand(&id,data_array,&ins);
if(status == ANDANTE_ERRBIT_NONE) {
CmdCheck((int16_t)id,data_array,ins);
}
-
+
}
/*******************************************************/
int main()
{
-
-
+
+
//Recieve.attach(&Rc,0.025);
//Start_Up();
@@ -679,9 +684,9 @@
SET_LowerPID();
while(1) {
- myled =0;
- //wait_ms(10);
-
+ myled =0;
+ //wait_ms(10);
+
///////////////////////////////////////////////////// start
//Set Set_point
Up_PID.setSetPoint(Upper_SetPoint);