Secret_Betago
first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
BEAR_Motion
by 
BE@R lab
Diff: main.cpp.orig
- Revision:
- 36:1561b6d61095
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig Wed Feb 03 14:49:46 2016 +0000
@@ -0,0 +1,774 @@
+//*****************************************************/
+// Include //
+#include "mbed.h"
+#include "pinconfig.h"
+#include "PID.h"
+#include "Motor.h"
+#include "eeprom.h"
+#include "Receiver.h"
+#include "Motion_EEPROM_Address.h"
+#define EEPROM_DELAY 2
+//*****************************************************/
+//--PID parameter--
+//-Upper-//
+float U_Kc;
+float U_Ki_gain;
+float U_Kd_gain;
+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_Ki_gain;
+float L_Kd_gain;
+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;
+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;
+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);
+//*****************************************************/
+
+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);
+ 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 *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_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
+ //printf("Up Angle = %f\n",up_angle);
+ IntLowAngle[0]=command[5];
+ IntLowAngle[1]=command[6];
+ Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
+ //printf("Low Angle = %f\n",low_angle);
+ break;
+ }
+ case UP_MARGIN: {
+ int i;
+ for(i=0; i<4; i++) {
+ UpMargin[i]=command[1+i];
+ //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
+ }
+ break;
+ }
+ case LOW_MARGIN: {
+ int i;
+ for(i=0; i<4; i++) {
+ LowMargin[i]=command[1+i];
+ }
+ 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);
+ 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/U_Kc;
+ //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;
+ 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) {
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
+ memory.write(ADDRESS_UP_MARGIN,data_buff);
+ 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(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: {
+ 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_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;
+ }
+ }
+ }
+ }
+}
+
+
+/******************************************************/
+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()
+{
+ 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\n\r");
+ while(1) {
+ myled =0;
+ //wait_ms(10);
+///////////////////////////////////////////////////// start
+ //Set Set_point
+ Up_PID.setSetPoint(Upper_SetPoint);
+ Low_PID.setSetPoint(Lower_SetPoint);
+
+ Read_Encoder(EncoderB);
+ Lower_Position = Get_EnValue(data);
+ PC.printf("position = %2f\n",Lower_Position);
+ //Control Motor
+ //Move_Upper();
+ //Move_Lower();
+///////////////////////////////////////////////////// stop =306us
+ //uint8_t aaa[1]={10};
+ //com.sendBodyWidth(0x01,aaa);
+ Rc();
+ //wait_ms(1);
+ }
+}
+
+
+
+
+
+