BE@R lab
โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
DogPID
by 
Digital B14
Diff: main.cpp
- Revision:
- 31:d6fa5e8e26b3
- Parent:
- 30:3f8e86fa1413
- Child:
- 32:1f81f3e83889
--- a/main.cpp Wed Jan 27 21:43:48 2016 +0000
+++ b/main.cpp Mon Feb 01 19:02:56 2016 +0000
@@ -7,7 +7,7 @@
#include "eeprom.h"
#include "Receiver.h"
#include "Motion_EEPROM_Address.h"
-
+#define EEPROM_DELAY 2
//*****************************************************/
//--PID parameter--
//-Upper-//
@@ -48,8 +48,8 @@
uint8_t LowLinkLength[4];
//-- encoder --
float up_angle,low_angle;
-int Upper_Position;
-int Lower_Position;
+float Upper_Position;
+float Lower_Position;
int data;
SPI ENC(Emosi, Emiso, Esck);
DigitalOut EncA(EncoderA);
@@ -130,7 +130,7 @@
void Move_Upper()
{
Read_Encoder(EncoderA);
- Upper_Position = Get_EnValue(data);
+ Upper_Position = (int)Get_EnValue(data);
Up_PID.setProcessValue(Upper_Position);
@@ -146,7 +146,7 @@
void Move_Lower()
{
Read_Encoder(EncoderB);
- Lower_Position = Get_EnValue(data);
+ Lower_Position = (int)Get_EnValue(data);
Low_PID.setProcessValue(Lower_Position);
@@ -177,24 +177,14 @@
case MOTOR_UPPER_ANG: {
uint8_t IntUpAngle[2],FloatUpAngle[2];
uint8_t IntLowAngle[2],FloatLowAngle[2];
- float int_buffer,float_buffer;
IntUpAngle[0]=command[1];
IntUpAngle[1]=command[2];
- FloatUpAngle[0]=command[3];
- FloatUpAngle[1]=command[4];
- int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
- float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
- up_angle=int_buffer+float_buffer;
+ Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
//printf("Up Angle = %f\n",up_angle);
-
IntLowAngle[0]=command[5];
IntLowAngle[1]=command[6];
- FloatLowAngle[0]=command[7];
- FloatLowAngle[1]=command[8];
- int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
- float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
- low_angle=int_buffer+float_buffer;
+ Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
//printf("Low Angle = %f\n",low_angle);
break;
}
@@ -374,13 +364,13 @@
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
memory.write(ADDRESS_HEIGHT,data_buff);
- wait_ms(1);
+ 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(1);
+ wait_ms(EEPROM_DELAY);
} else if(command[1]==OFFSET) {
uint8_t y_offset_array[4];
@@ -393,9 +383,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);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_OFFSET+4,z_data_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
} else if(command[1]==MAG_DATA) {
uint8_t x_max_array[4];
@@ -420,17 +410,17 @@
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);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_MAG_DATA+4,x_min_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_MAG_DATA+8,y_max_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_MAG_DATA+12,y_min_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_MAG_DATA+16,z_max_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
}
@@ -438,42 +428,42 @@
// else {
if (command[1]==ID) {
memory.write(ADDRESS_ID,id);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
} else if(command[1]==UP_MARGIN) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
memory.write(ADDRESS_UP_MARGIN,data_buff);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
//printf("save OK!!\n\r");
} else if (command[1]==LOW_MARGIN) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin);
memory.write(ADDRESS_LOW_MARGIN,data_buff);
- wait_ms(1);
+ 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(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_UPPER_KI,U_Ki_gain);
//printf("U_Write : %f\r\n",U_Ki_gain);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_UPPER_KD,U_Kd_gain);
//printf("U_Write : %f\r\n",U_Kd_gain);
- wait_ms(1);
+ 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(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_LOWER_KI,L_Ki_gain);
//printf("L_Write : %f\r\n",L_Ki_gain);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_LOWER_KD,L_Kd_gain);
//printf("L_Write : %f\r\n",L_Kd_gain);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
} else if (command[1]==ANGLE_RANGE_UP) {
uint8_t max_array[4];
@@ -486,9 +476,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);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
- wait_ms(1);
+ wait_ms(EEPROM_DELAY);
} else if (command[1]==ANGLE_RANGE_LOW) {
uint8_t max_array[4];
@@ -501,27 +491,27 @@
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);
+ wait_ms(EEPROM_DELAY);
memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
- wait_ms(1);
+ 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(1);
+ 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(1);
+ 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(1);
+ wait_ms(EEPROM_DELAY);
}
break;
}
@@ -532,7 +522,7 @@
case READ_DATA: {
switch (command[0]) {
case MOTOR_UPPER_ANG: {
- com.sendMotorPos(MY_ID,up_angle,low_angle);
+ com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
break;
}
case UP_MARGIN: {
@@ -727,6 +717,19 @@
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();