first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
main.cpp.orig@37:8719223998d8, 2016-06-07 (annotated)
- Committer:
- icyzkungz
- Date:
- Tue Jun 07 07:00:11 2016 +0000
- Revision:
- 37:8719223998d8
- Parent:
- 36:1561b6d61095
update protocol.h
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
soulx | 36:1561b6d61095 | 1 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 2 | // Include // |
soulx | 36:1561b6d61095 | 3 | #include "mbed.h" |
soulx | 36:1561b6d61095 | 4 | #include "pinconfig.h" |
soulx | 36:1561b6d61095 | 5 | #include "PID.h" |
soulx | 36:1561b6d61095 | 6 | #include "Motor.h" |
soulx | 36:1561b6d61095 | 7 | #include "eeprom.h" |
soulx | 36:1561b6d61095 | 8 | #include "Receiver.h" |
soulx | 36:1561b6d61095 | 9 | #include "Motion_EEPROM_Address.h" |
soulx | 36:1561b6d61095 | 10 | #define EEPROM_DELAY 2 |
soulx | 36:1561b6d61095 | 11 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 12 | //--PID parameter-- |
soulx | 36:1561b6d61095 | 13 | //-Upper-// |
soulx | 36:1561b6d61095 | 14 | float U_Kc; |
soulx | 36:1561b6d61095 | 15 | float U_Ki_gain; |
soulx | 36:1561b6d61095 | 16 | float U_Kd_gain; |
soulx | 36:1561b6d61095 | 17 | float U_Ti; |
soulx | 36:1561b6d61095 | 18 | float U_Td; |
soulx | 36:1561b6d61095 | 19 | float U_Ki=U_Kc*U_Ti; |
soulx | 36:1561b6d61095 | 20 | float U_Kd=U_Kc*U_Td; |
soulx | 36:1561b6d61095 | 21 | //-lower-// |
soulx | 36:1561b6d61095 | 22 | float L_Kc; |
soulx | 36:1561b6d61095 | 23 | float L_Ki_gain; |
soulx | 36:1561b6d61095 | 24 | float L_Kd_gain; |
soulx | 36:1561b6d61095 | 25 | float L_Ti; |
soulx | 36:1561b6d61095 | 26 | float L_Td; |
soulx | 36:1561b6d61095 | 27 | float L_Ki=L_Kc*L_Ti; |
soulx | 36:1561b6d61095 | 28 | float L_Kd=L_Kc*L_Td; |
soulx | 36:1561b6d61095 | 29 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 30 | // Global // |
soulx | 36:1561b6d61095 | 31 | Ticker Recieve; |
soulx | 36:1561b6d61095 | 32 | //-- Communication -- |
soulx | 36:1561b6d61095 | 33 | Serial PC(D1,D0); |
soulx | 36:1561b6d61095 | 34 | Bear_Receiver com(Tx,Rx,1000000); |
soulx | 36:1561b6d61095 | 35 | int16_t MY_ID = 0x01; |
soulx | 36:1561b6d61095 | 36 | //-- Memorry -- |
soulx | 36:1561b6d61095 | 37 | EEPROM memory(PB_4,PA_8,0); |
soulx | 36:1561b6d61095 | 38 | uint8_t UpMargin[4]; |
soulx | 36:1561b6d61095 | 39 | uint8_t LowMargin[4]; |
soulx | 36:1561b6d61095 | 40 | uint8_t Height[4]; |
soulx | 36:1561b6d61095 | 41 | uint8_t Wheelpos[4]; |
soulx | 36:1561b6d61095 | 42 | uint8_t Mag[24]; |
soulx | 36:1561b6d61095 | 43 | uint8_t Offset[8];//={1,2,3,4,5,6,7,8}; |
soulx | 36:1561b6d61095 | 44 | uint8_t Body_width[4]; |
soulx | 36:1561b6d61095 | 45 | uint8_t Angle_Range_Up[8]; |
soulx | 36:1561b6d61095 | 46 | uint8_t Angle_Range_Low[8]; |
soulx | 36:1561b6d61095 | 47 | uint8_t UpLinkLength[4]; |
soulx | 36:1561b6d61095 | 48 | uint8_t LowLinkLength[4]; |
soulx | 36:1561b6d61095 | 49 | //-- encoder -- |
soulx | 36:1561b6d61095 | 50 | float up_angle,low_angle; |
soulx | 36:1561b6d61095 | 51 | float Upper_Position; |
soulx | 36:1561b6d61095 | 52 | float Lower_Position; |
soulx | 36:1561b6d61095 | 53 | int data; |
soulx | 36:1561b6d61095 | 54 | SPI ENC(Emosi, Emiso, Esck); |
soulx | 36:1561b6d61095 | 55 | DigitalOut EncA(EncoderA); |
soulx | 36:1561b6d61095 | 56 | DigitalOut EncB(EncoderB); |
soulx | 36:1561b6d61095 | 57 | //-- Motor -- |
soulx | 36:1561b6d61095 | 58 | int dir; |
soulx | 36:1561b6d61095 | 59 | Motor Upper(PWM_LU,A_LU,B_LU); |
soulx | 36:1561b6d61095 | 60 | Motor Lower(PWM_LL,A_LL,B_LL); |
soulx | 36:1561b6d61095 | 61 | //-- PID -- |
soulx | 36:1561b6d61095 | 62 | int Upper_SetPoint; |
soulx | 36:1561b6d61095 | 63 | int Lower_SetPoint; |
soulx | 36:1561b6d61095 | 64 | PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate |
soulx | 36:1561b6d61095 | 65 | PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); |
soulx | 36:1561b6d61095 | 66 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 67 | |
soulx | 36:1561b6d61095 | 68 | DigitalOut myled(LED1); |
soulx | 36:1561b6d61095 | 69 | |
soulx | 36:1561b6d61095 | 70 | |
soulx | 36:1561b6d61095 | 71 | void Read_Encoder(PinName Encoder) |
soulx | 36:1561b6d61095 | 72 | { |
soulx | 36:1561b6d61095 | 73 | ENC.format(8,0); |
soulx | 36:1561b6d61095 | 74 | ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k |
soulx | 36:1561b6d61095 | 75 | |
soulx | 36:1561b6d61095 | 76 | if(Encoder == EncoderA) { |
soulx | 36:1561b6d61095 | 77 | EncA = 0; |
soulx | 36:1561b6d61095 | 78 | } else { |
soulx | 36:1561b6d61095 | 79 | EncB = 0; |
soulx | 36:1561b6d61095 | 80 | } |
soulx | 36:1561b6d61095 | 81 | ENC.write(0x41); |
soulx | 36:1561b6d61095 | 82 | ENC.write(0x09); |
soulx | 36:1561b6d61095 | 83 | data = ENC.write(0x00); |
soulx | 36:1561b6d61095 | 84 | if(Encoder == EncoderA) { |
soulx | 36:1561b6d61095 | 85 | EncA = 1; |
soulx | 36:1561b6d61095 | 86 | } else { |
soulx | 36:1561b6d61095 | 87 | EncB = 1; |
soulx | 36:1561b6d61095 | 88 | } |
soulx | 36:1561b6d61095 | 89 | |
soulx | 36:1561b6d61095 | 90 | } |
soulx | 36:1561b6d61095 | 91 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 92 | int Get_EnValue(int Val) |
soulx | 36:1561b6d61095 | 93 | { |
soulx | 36:1561b6d61095 | 94 | int i = 0; |
soulx | 36:1561b6d61095 | 95 | static unsigned char codes[] = { |
soulx | 36:1561b6d61095 | 96 | 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, |
soulx | 36:1561b6d61095 | 97 | 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, |
soulx | 36:1561b6d61095 | 98 | 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, |
soulx | 36:1561b6d61095 | 99 | 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, |
soulx | 36:1561b6d61095 | 100 | 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, |
soulx | 36:1561b6d61095 | 101 | 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, |
soulx | 36:1561b6d61095 | 102 | 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, |
soulx | 36:1561b6d61095 | 103 | 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 |
soulx | 36:1561b6d61095 | 104 | }; |
soulx | 36:1561b6d61095 | 105 | if ( MY_ID == 0x01 )//when it was left side |
soulx | 36:1561b6d61095 | 106 | { |
soulx | 36:1561b6d61095 | 107 | while(Val != codes[i]) { |
soulx | 36:1561b6d61095 | 108 | i++; |
soulx | 36:1561b6d61095 | 109 | } |
soulx | 36:1561b6d61095 | 110 | } |
soulx | 36:1561b6d61095 | 111 | if ( MY_ID == 0x02 )//when it was right side |
soulx | 36:1561b6d61095 | 112 | { |
soulx | 36:1561b6d61095 | 113 | while(Val != codes[127-i]) { |
soulx | 36:1561b6d61095 | 114 | i++; |
soulx | 36:1561b6d61095 | 115 | } |
soulx | 36:1561b6d61095 | 116 | } |
soulx | 36:1561b6d61095 | 117 | return i; |
soulx | 36:1561b6d61095 | 118 | |
soulx | 36:1561b6d61095 | 119 | } |
soulx | 36:1561b6d61095 | 120 | //*****************************************************/ |
soulx | 36:1561b6d61095 | 121 | void SET_UpperPID() |
soulx | 36:1561b6d61095 | 122 | { |
soulx | 36:1561b6d61095 | 123 | Upper.period(0.001); |
soulx | 36:1561b6d61095 | 124 | Up_PID.setMode(0); |
soulx | 36:1561b6d61095 | 125 | Up_PID.setInputLimits(0,127); |
soulx | 36:1561b6d61095 | 126 | Up_PID.setOutputLimits(0,1); |
soulx | 36:1561b6d61095 | 127 | } |
soulx | 36:1561b6d61095 | 128 | //******************************************************/ |
soulx | 36:1561b6d61095 | 129 | void SET_LowerPID() |
soulx | 36:1561b6d61095 | 130 | { |
soulx | 36:1561b6d61095 | 131 | Lower.period(0.001); |
soulx | 36:1561b6d61095 | 132 | Low_PID.setMode(0); |
soulx | 36:1561b6d61095 | 133 | Low_PID.setInputLimits(0,127); // set range |
soulx | 36:1561b6d61095 | 134 | Low_PID.setOutputLimits(0,1); |
soulx | 36:1561b6d61095 | 135 | } |
soulx | 36:1561b6d61095 | 136 | //******************************************************/ |
soulx | 36:1561b6d61095 | 137 | void Move_Upper() |
soulx | 36:1561b6d61095 | 138 | { |
soulx | 36:1561b6d61095 | 139 | Read_Encoder(EncoderA); |
soulx | 36:1561b6d61095 | 140 | Upper_Position = Get_EnValue(data); |
soulx | 36:1561b6d61095 | 141 | |
soulx | 36:1561b6d61095 | 142 | Up_PID.setProcessValue(Upper_Position); |
soulx | 36:1561b6d61095 | 143 | |
soulx | 36:1561b6d61095 | 144 | if(Upper_Position - Upper_SetPoint > 0 ) { |
soulx | 36:1561b6d61095 | 145 | dir = 1; |
soulx | 36:1561b6d61095 | 146 | } |
soulx | 36:1561b6d61095 | 147 | if(Upper_Position - Upper_SetPoint < 0 ) { |
soulx | 36:1561b6d61095 | 148 | dir = -1; |
soulx | 36:1561b6d61095 | 149 | } |
soulx | 36:1561b6d61095 | 150 | Upper.speed(Up_PID.compute() * dir); |
soulx | 36:1561b6d61095 | 151 | } |
soulx | 36:1561b6d61095 | 152 | //******************************************************/ |
soulx | 36:1561b6d61095 | 153 | void Move_Lower() |
soulx | 36:1561b6d61095 | 154 | { |
soulx | 36:1561b6d61095 | 155 | Read_Encoder(EncoderB); |
soulx | 36:1561b6d61095 | 156 | Lower_Position = Get_EnValue(data); |
soulx | 36:1561b6d61095 | 157 | |
soulx | 36:1561b6d61095 | 158 | Low_PID.setProcessValue(Lower_Position); |
soulx | 36:1561b6d61095 | 159 | |
soulx | 36:1561b6d61095 | 160 | if(Lower_Position - Lower_SetPoint > 0 ) { |
soulx | 36:1561b6d61095 | 161 | dir = 1; |
soulx | 36:1561b6d61095 | 162 | } |
soulx | 36:1561b6d61095 | 163 | if(Lower_Position - Lower_SetPoint < 0 ) { |
soulx | 36:1561b6d61095 | 164 | dir = -1; |
soulx | 36:1561b6d61095 | 165 | } |
soulx | 36:1561b6d61095 | 166 | Lower.speed(Low_PID.compute() * dir); |
soulx | 36:1561b6d61095 | 167 | } |
soulx | 36:1561b6d61095 | 168 | //******************************************************/ |
soulx | 36:1561b6d61095 | 169 | |
soulx | 36:1561b6d61095 | 170 | void CmdCheck(int16_t id,uint8_t *command,uint8_t ins) |
soulx | 36:1561b6d61095 | 171 | { |
soulx | 36:1561b6d61095 | 172 | if(id==MY_ID) { |
soulx | 36:1561b6d61095 | 173 | switch (ins) { |
soulx | 36:1561b6d61095 | 174 | case PING: { |
soulx | 36:1561b6d61095 | 175 | break; |
soulx | 36:1561b6d61095 | 176 | } |
soulx | 36:1561b6d61095 | 177 | case WRITE_DATA: { |
soulx | 36:1561b6d61095 | 178 | switch (command[0]) { |
soulx | 36:1561b6d61095 | 179 | case ID: { |
soulx | 36:1561b6d61095 | 180 | /// |
soulx | 36:1561b6d61095 | 181 | MY_ID = (int16_t)command[1]; |
soulx | 36:1561b6d61095 | 182 | break; |
soulx | 36:1561b6d61095 | 183 | } |
soulx | 36:1561b6d61095 | 184 | case MOTOR_UPPER_ANG: { |
soulx | 36:1561b6d61095 | 185 | uint8_t IntUpAngle[2]; |
soulx | 36:1561b6d61095 | 186 | uint8_t IntLowAngle[2]; |
soulx | 36:1561b6d61095 | 187 | |
soulx | 36:1561b6d61095 | 188 | IntUpAngle[0]=command[1]; |
soulx | 36:1561b6d61095 | 189 | IntUpAngle[1]=command[2]; |
soulx | 36:1561b6d61095 | 190 | Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); |
soulx | 36:1561b6d61095 | 191 | //printf("Up Angle = %f\n",up_angle); |
soulx | 36:1561b6d61095 | 192 | IntLowAngle[0]=command[5]; |
soulx | 36:1561b6d61095 | 193 | IntLowAngle[1]=command[6]; |
soulx | 36:1561b6d61095 | 194 | Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); |
soulx | 36:1561b6d61095 | 195 | //printf("Low Angle = %f\n",low_angle); |
soulx | 36:1561b6d61095 | 196 | break; |
soulx | 36:1561b6d61095 | 197 | } |
soulx | 36:1561b6d61095 | 198 | case UP_MARGIN: { |
soulx | 36:1561b6d61095 | 199 | int i; |
soulx | 36:1561b6d61095 | 200 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 201 | UpMargin[i]=command[1+i]; |
soulx | 36:1561b6d61095 | 202 | //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]); |
soulx | 36:1561b6d61095 | 203 | } |
soulx | 36:1561b6d61095 | 204 | break; |
soulx | 36:1561b6d61095 | 205 | } |
soulx | 36:1561b6d61095 | 206 | case LOW_MARGIN: { |
soulx | 36:1561b6d61095 | 207 | int i; |
soulx | 36:1561b6d61095 | 208 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 209 | LowMargin[i]=command[1+i]; |
soulx | 36:1561b6d61095 | 210 | } |
soulx | 36:1561b6d61095 | 211 | break; |
soulx | 36:1561b6d61095 | 212 | } |
soulx | 36:1561b6d61095 | 213 | case KP_UPPER_MOTOR: { |
soulx | 36:1561b6d61095 | 214 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 215 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 216 | float Int,Float; |
soulx | 36:1561b6d61095 | 217 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 218 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 219 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 220 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 221 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 222 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 223 | U_Kc=Int+Float; |
soulx | 36:1561b6d61095 | 224 | //printf("Kp Upper : %f\r\n",U_Kc); |
soulx | 36:1561b6d61095 | 225 | break; |
soulx | 36:1561b6d61095 | 226 | } |
soulx | 36:1561b6d61095 | 227 | case KI_UPPER_MOTOR: { |
soulx | 36:1561b6d61095 | 228 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 229 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 230 | float Int,Float; |
soulx | 36:1561b6d61095 | 231 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 232 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 233 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 234 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 235 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 236 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 237 | U_Ki_gain=Int+Float; |
soulx | 36:1561b6d61095 | 238 | U_Ti=U_Ki_gain/U_Kc; |
soulx | 36:1561b6d61095 | 239 | //printf("Ki Upper : %f\r\n",U_Ki_gain); |
soulx | 36:1561b6d61095 | 240 | break; |
soulx | 36:1561b6d61095 | 241 | } |
soulx | 36:1561b6d61095 | 242 | case KD_UPPER_MOTOR: { |
soulx | 36:1561b6d61095 | 243 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 244 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 245 | float Int,Float; |
soulx | 36:1561b6d61095 | 246 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 247 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 248 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 249 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 250 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 251 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 252 | U_Kd_gain=Int+Float; |
soulx | 36:1561b6d61095 | 253 | U_Td=U_Kd_gain/U_Kc; |
soulx | 36:1561b6d61095 | 254 | //printf("Kd Upper : %f\r\n",U_Kd_gain); |
soulx | 36:1561b6d61095 | 255 | break; |
soulx | 36:1561b6d61095 | 256 | } |
soulx | 36:1561b6d61095 | 257 | case KP_LOWER_MOTOR: { |
soulx | 36:1561b6d61095 | 258 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 259 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 260 | float Int,Float; |
soulx | 36:1561b6d61095 | 261 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 262 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 263 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 264 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 265 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 266 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 267 | L_Kc=Int+Float; |
soulx | 36:1561b6d61095 | 268 | //printf("Kp Lower : %f\r\n",L_Kc); |
soulx | 36:1561b6d61095 | 269 | break; |
soulx | 36:1561b6d61095 | 270 | } |
soulx | 36:1561b6d61095 | 271 | case KI_LOWER_MOTOR: { |
soulx | 36:1561b6d61095 | 272 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 273 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 274 | float Int,Float; |
soulx | 36:1561b6d61095 | 275 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 276 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 277 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 278 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 279 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 280 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 281 | L_Ki_gain=Int+Float; |
soulx | 36:1561b6d61095 | 282 | L_Ti=L_Ki_gain/L_Kc; |
soulx | 36:1561b6d61095 | 283 | //printf("Ki Lower : %f\r\n",L_Ki_gain); |
soulx | 36:1561b6d61095 | 284 | break; |
soulx | 36:1561b6d61095 | 285 | } |
soulx | 36:1561b6d61095 | 286 | case KD_LOWER_MOTOR: { |
soulx | 36:1561b6d61095 | 287 | uint8_t int_buffer[2]; |
soulx | 36:1561b6d61095 | 288 | uint8_t float_buffer[2]; |
soulx | 36:1561b6d61095 | 289 | float Int,Float; |
soulx | 36:1561b6d61095 | 290 | int_buffer[0]=command[1]; |
soulx | 36:1561b6d61095 | 291 | int_buffer[1]=command[2]; |
soulx | 36:1561b6d61095 | 292 | float_buffer[0]=command[3]; |
soulx | 36:1561b6d61095 | 293 | float_buffer[1]=command[4]; |
soulx | 36:1561b6d61095 | 294 | Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); |
soulx | 36:1561b6d61095 | 295 | Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; |
soulx | 36:1561b6d61095 | 296 | L_Kd_gain=Int+Float; |
soulx | 36:1561b6d61095 | 297 | L_Td=L_Kd_gain/L_Kc; |
soulx | 36:1561b6d61095 | 298 | //printf("Kd Lower : %f\r\n",L_Kd_gain); |
soulx | 36:1561b6d61095 | 299 | break; |
soulx | 36:1561b6d61095 | 300 | } |
soulx | 36:1561b6d61095 | 301 | case HEIGHT: { |
soulx | 36:1561b6d61095 | 302 | int i; |
soulx | 36:1561b6d61095 | 303 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 304 | Height[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 305 | } |
soulx | 36:1561b6d61095 | 306 | break; |
soulx | 36:1561b6d61095 | 307 | } |
soulx | 36:1561b6d61095 | 308 | case WHEELPOS: { |
soulx | 36:1561b6d61095 | 309 | int i; |
soulx | 36:1561b6d61095 | 310 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 311 | Wheelpos[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 312 | } |
soulx | 36:1561b6d61095 | 313 | break; |
soulx | 36:1561b6d61095 | 314 | } |
soulx | 36:1561b6d61095 | 315 | case MAG_DATA: { |
soulx | 36:1561b6d61095 | 316 | int i; |
soulx | 36:1561b6d61095 | 317 | for(i=0; i<24; i++) { |
soulx | 36:1561b6d61095 | 318 | Mag[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 319 | } |
soulx | 36:1561b6d61095 | 320 | break; |
soulx | 36:1561b6d61095 | 321 | } |
soulx | 36:1561b6d61095 | 322 | case OFFSET: { |
soulx | 36:1561b6d61095 | 323 | int i; |
soulx | 36:1561b6d61095 | 324 | for(i=0; i<8; i++) { |
soulx | 36:1561b6d61095 | 325 | Offset[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 326 | } |
soulx | 36:1561b6d61095 | 327 | break; |
soulx | 36:1561b6d61095 | 328 | } |
soulx | 36:1561b6d61095 | 329 | case BODY_WIDTH: { |
soulx | 36:1561b6d61095 | 330 | int i; |
soulx | 36:1561b6d61095 | 331 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 332 | Body_width[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 333 | } |
soulx | 36:1561b6d61095 | 334 | break; |
soulx | 36:1561b6d61095 | 335 | } |
soulx | 36:1561b6d61095 | 336 | case ANGLE_RANGE_UP: { |
soulx | 36:1561b6d61095 | 337 | int i; |
soulx | 36:1561b6d61095 | 338 | for(i=0; i<8; i++) { |
soulx | 36:1561b6d61095 | 339 | Angle_Range_Up[i]=command[1+i]; |
soulx | 36:1561b6d61095 | 340 | //printf("%d Angle = 0x%02x\r\n",i,Angle_Range_Up[i]); |
soulx | 36:1561b6d61095 | 341 | } |
soulx | 36:1561b6d61095 | 342 | break; |
soulx | 36:1561b6d61095 | 343 | } |
soulx | 36:1561b6d61095 | 344 | case ANGLE_RANGE_LOW: { |
soulx | 36:1561b6d61095 | 345 | int i; |
soulx | 36:1561b6d61095 | 346 | for(i=0; i<8; i++) { |
soulx | 36:1561b6d61095 | 347 | Angle_Range_Low[0+i]=command[1+i]; |
soulx | 36:1561b6d61095 | 348 | } |
soulx | 36:1561b6d61095 | 349 | break; |
soulx | 36:1561b6d61095 | 350 | } |
soulx | 36:1561b6d61095 | 351 | |
soulx | 36:1561b6d61095 | 352 | case UP_LINK_LENGTH: { |
soulx | 36:1561b6d61095 | 353 | int i; |
soulx | 36:1561b6d61095 | 354 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 355 | UpLinkLength[i]=command[1+i]; |
soulx | 36:1561b6d61095 | 356 | } |
soulx | 36:1561b6d61095 | 357 | break; |
soulx | 36:1561b6d61095 | 358 | } |
soulx | 36:1561b6d61095 | 359 | case LOW_LINK_LENGTH: { |
soulx | 36:1561b6d61095 | 360 | int i; |
soulx | 36:1561b6d61095 | 361 | for(i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 362 | LowLinkLength[i]=command[1+i]; |
soulx | 36:1561b6d61095 | 363 | } |
soulx | 36:1561b6d61095 | 364 | break; |
soulx | 36:1561b6d61095 | 365 | } |
soulx | 36:1561b6d61095 | 366 | // unfinish yet!!!!!!!!!!!!!!!!! |
soulx | 36:1561b6d61095 | 367 | case SAVE_EEPROM_DATA: { |
soulx | 36:1561b6d61095 | 368 | if(id==0x01) { |
soulx | 36:1561b6d61095 | 369 | |
soulx | 36:1561b6d61095 | 370 | if (command[1]==HEIGHT) { |
soulx | 36:1561b6d61095 | 371 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 372 | data_buff = Utilities::ConvertUInt8ArrayToInt32(Height); |
soulx | 36:1561b6d61095 | 373 | memory.write(ADDRESS_HEIGHT,data_buff); |
soulx | 36:1561b6d61095 | 374 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 375 | |
soulx | 36:1561b6d61095 | 376 | } else if(command[1]==BODY_WIDTH) { |
soulx | 36:1561b6d61095 | 377 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 378 | data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width); |
soulx | 36:1561b6d61095 | 379 | memory.write(ADDRESS_BODY_WIDTH,data_buff); |
soulx | 36:1561b6d61095 | 380 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 381 | |
soulx | 36:1561b6d61095 | 382 | } else if(command[1]==OFFSET) { |
soulx | 36:1561b6d61095 | 383 | uint8_t y_offset_array[4]; |
soulx | 36:1561b6d61095 | 384 | uint8_t z_offset_array[4]; |
soulx | 36:1561b6d61095 | 385 | int32_t y_data_buffer,z_data_buffer; |
soulx | 36:1561b6d61095 | 386 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 387 | y_offset_array[i]=Offset[i]; |
soulx | 36:1561b6d61095 | 388 | z_offset_array[i]=Offset[i+4]; |
soulx | 36:1561b6d61095 | 389 | } |
soulx | 36:1561b6d61095 | 390 | y_data_buffer = Utilities::ConvertUInt8ArrayToInt32(y_offset_array); |
soulx | 36:1561b6d61095 | 391 | z_data_buffer = Utilities::ConvertUInt8ArrayToInt32(z_offset_array); |
soulx | 36:1561b6d61095 | 392 | memory.write(ADDRESS_OFFSET,y_data_buffer); |
soulx | 36:1561b6d61095 | 393 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 394 | memory.write(ADDRESS_OFFSET+4,z_data_buffer); |
soulx | 36:1561b6d61095 | 395 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 396 | |
soulx | 36:1561b6d61095 | 397 | } else if(command[1]==MAG_DATA) { |
soulx | 36:1561b6d61095 | 398 | uint8_t x_max_array[4]; |
soulx | 36:1561b6d61095 | 399 | uint8_t x_min_array[4]; |
soulx | 36:1561b6d61095 | 400 | uint8_t y_max_array[4]; |
soulx | 36:1561b6d61095 | 401 | uint8_t y_min_array[4]; |
soulx | 36:1561b6d61095 | 402 | uint8_t z_max_array[4]; |
soulx | 36:1561b6d61095 | 403 | uint8_t z_min_array[4]; |
soulx | 36:1561b6d61095 | 404 | int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer; |
soulx | 36:1561b6d61095 | 405 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 406 | x_max_array[i]=Mag[i]; |
soulx | 36:1561b6d61095 | 407 | x_min_array[i]=Mag[i+4]; |
soulx | 36:1561b6d61095 | 408 | y_max_array[i]=Mag[i+8]; |
soulx | 36:1561b6d61095 | 409 | y_min_array[i]=Mag[i+12]; |
soulx | 36:1561b6d61095 | 410 | z_max_array[i]=Mag[i+16]; |
soulx | 36:1561b6d61095 | 411 | z_min_array[i]=Mag[i+20]; |
soulx | 36:1561b6d61095 | 412 | } |
soulx | 36:1561b6d61095 | 413 | x_max_buffer = Utilities::ConvertUInt8ArrayToInt32(x_max_array); |
soulx | 36:1561b6d61095 | 414 | x_min_buffer = Utilities::ConvertUInt8ArrayToInt32(x_min_array); |
soulx | 36:1561b6d61095 | 415 | y_max_buffer = Utilities::ConvertUInt8ArrayToInt32(y_max_array); |
soulx | 36:1561b6d61095 | 416 | y_min_buffer = Utilities::ConvertUInt8ArrayToInt32(y_min_array); |
soulx | 36:1561b6d61095 | 417 | z_max_buffer = Utilities::ConvertUInt8ArrayToInt32(z_max_array); |
soulx | 36:1561b6d61095 | 418 | z_min_buffer = Utilities::ConvertUInt8ArrayToInt32(z_min_array); |
soulx | 36:1561b6d61095 | 419 | memory.write(ADDRESS_MAG_DATA,x_max_buffer); |
soulx | 36:1561b6d61095 | 420 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 421 | memory.write(ADDRESS_MAG_DATA+4,x_min_buffer); |
soulx | 36:1561b6d61095 | 422 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 423 | memory.write(ADDRESS_MAG_DATA+8,y_max_buffer); |
soulx | 36:1561b6d61095 | 424 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 425 | memory.write(ADDRESS_MAG_DATA+12,y_min_buffer); |
soulx | 36:1561b6d61095 | 426 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 427 | memory.write(ADDRESS_MAG_DATA+16,z_max_buffer); |
soulx | 36:1561b6d61095 | 428 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 429 | memory.write(ADDRESS_MAG_DATA+20,z_min_buffer); |
soulx | 36:1561b6d61095 | 430 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 431 | |
soulx | 36:1561b6d61095 | 432 | } |
soulx | 36:1561b6d61095 | 433 | |
soulx | 36:1561b6d61095 | 434 | } |
soulx | 36:1561b6d61095 | 435 | // else { |
soulx | 36:1561b6d61095 | 436 | if (command[1]==ID) { |
soulx | 36:1561b6d61095 | 437 | memory.write(ADDRESS_ID,id); |
soulx | 36:1561b6d61095 | 438 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 439 | |
soulx | 36:1561b6d61095 | 440 | } else if(command[1]==UP_MARGIN) { |
soulx | 36:1561b6d61095 | 441 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 442 | data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); |
soulx | 36:1561b6d61095 | 443 | memory.write(ADDRESS_UP_MARGIN,data_buff); |
soulx | 36:1561b6d61095 | 444 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 445 | //printf("save OK!!\n\r"); |
soulx | 36:1561b6d61095 | 446 | |
soulx | 36:1561b6d61095 | 447 | } else if (command[1]==LOW_MARGIN) { |
soulx | 36:1561b6d61095 | 448 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 449 | data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); |
soulx | 36:1561b6d61095 | 450 | memory.write(ADDRESS_LOW_MARGIN,data_buff); |
soulx | 36:1561b6d61095 | 451 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 452 | |
soulx | 36:1561b6d61095 | 453 | } else if (command[1]==PID_UPPER_MOTOR) { |
soulx | 36:1561b6d61095 | 454 | memory.write(ADDRESS_UPPER_KP,U_Kc); |
soulx | 36:1561b6d61095 | 455 | //printf("U_Write : %f\r\n",U_Kc); |
soulx | 36:1561b6d61095 | 456 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 457 | memory.write(ADDRESS_UPPER_KI,U_Ki_gain); |
soulx | 36:1561b6d61095 | 458 | //printf("U_Write : %f\r\n",U_Ki_gain); |
soulx | 36:1561b6d61095 | 459 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 460 | memory.write(ADDRESS_UPPER_KD,U_Kd_gain); |
soulx | 36:1561b6d61095 | 461 | //printf("U_Write : %f\r\n",U_Kd_gain); |
soulx | 36:1561b6d61095 | 462 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 463 | |
soulx | 36:1561b6d61095 | 464 | } else if (command[1]==PID_LOWER_MOTOR) { |
soulx | 36:1561b6d61095 | 465 | memory.write(ADDRESS_LOWER_KP,L_Kc); |
soulx | 36:1561b6d61095 | 466 | //printf("L_Write : %f\r\n",L_Kc); |
soulx | 36:1561b6d61095 | 467 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 468 | memory.write(ADDRESS_LOWER_KI,L_Ki_gain); |
soulx | 36:1561b6d61095 | 469 | //printf("L_Write : %f\r\n",L_Ki_gain); |
soulx | 36:1561b6d61095 | 470 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 471 | memory.write(ADDRESS_LOWER_KD,L_Kd_gain); |
soulx | 36:1561b6d61095 | 472 | //printf("L_Write : %f\r\n",L_Kd_gain); |
soulx | 36:1561b6d61095 | 473 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 474 | |
soulx | 36:1561b6d61095 | 475 | } else if (command[1]==ANGLE_RANGE_UP) { |
soulx | 36:1561b6d61095 | 476 | uint8_t max_array[4]; |
soulx | 36:1561b6d61095 | 477 | uint8_t min_array[4]; |
soulx | 36:1561b6d61095 | 478 | int32_t max_data_buffer,min_data_buffer; |
soulx | 36:1561b6d61095 | 479 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 480 | max_array[i]=Angle_Range_Up[i]; |
soulx | 36:1561b6d61095 | 481 | min_array[i]=Angle_Range_Up[i+4]; |
soulx | 36:1561b6d61095 | 482 | } |
soulx | 36:1561b6d61095 | 483 | max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); |
soulx | 36:1561b6d61095 | 484 | min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); |
soulx | 36:1561b6d61095 | 485 | memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); |
soulx | 36:1561b6d61095 | 486 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 487 | memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); |
soulx | 36:1561b6d61095 | 488 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 489 | |
soulx | 36:1561b6d61095 | 490 | } else if (command[1]==ANGLE_RANGE_LOW) { |
soulx | 36:1561b6d61095 | 491 | uint8_t max_array[4]; |
soulx | 36:1561b6d61095 | 492 | uint8_t min_array[4]; |
soulx | 36:1561b6d61095 | 493 | int32_t max_data_buffer,min_data_buffer; |
soulx | 36:1561b6d61095 | 494 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 495 | max_array[i]=Angle_Range_Low[i]; |
soulx | 36:1561b6d61095 | 496 | min_array[i]=Angle_Range_Low[i+4]; |
soulx | 36:1561b6d61095 | 497 | } |
soulx | 36:1561b6d61095 | 498 | max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); |
soulx | 36:1561b6d61095 | 499 | min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); |
soulx | 36:1561b6d61095 | 500 | memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); |
soulx | 36:1561b6d61095 | 501 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 502 | memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); |
soulx | 36:1561b6d61095 | 503 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 504 | |
soulx | 36:1561b6d61095 | 505 | } else if (command[1]==UP_LINK_LENGTH) { |
soulx | 36:1561b6d61095 | 506 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 507 | data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); |
soulx | 36:1561b6d61095 | 508 | memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); |
soulx | 36:1561b6d61095 | 509 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 510 | |
soulx | 36:1561b6d61095 | 511 | } else if (command[1]==LOW_LINK_LENGTH) { |
soulx | 36:1561b6d61095 | 512 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 513 | data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); |
soulx | 36:1561b6d61095 | 514 | memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); |
soulx | 36:1561b6d61095 | 515 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 516 | |
soulx | 36:1561b6d61095 | 517 | } else if (command[1]==WHEELPOS) { |
soulx | 36:1561b6d61095 | 518 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 519 | data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); |
soulx | 36:1561b6d61095 | 520 | memory.write(ADDRESS_WHEELPOS,data_buff); |
soulx | 36:1561b6d61095 | 521 | wait_ms(EEPROM_DELAY); |
soulx | 36:1561b6d61095 | 522 | } |
soulx | 36:1561b6d61095 | 523 | break; |
soulx | 36:1561b6d61095 | 524 | } |
soulx | 36:1561b6d61095 | 525 | break; |
soulx | 36:1561b6d61095 | 526 | } |
soulx | 36:1561b6d61095 | 527 | break; |
soulx | 36:1561b6d61095 | 528 | } |
soulx | 36:1561b6d61095 | 529 | case READ_DATA: { |
soulx | 36:1561b6d61095 | 530 | switch (command[0]) { |
soulx | 36:1561b6d61095 | 531 | case MOTOR_UPPER_ANG: { |
soulx | 36:1561b6d61095 | 532 | com.sendMotorPos(MY_ID,Upper_Position,Lower_Position); |
soulx | 36:1561b6d61095 | 533 | break; |
soulx | 36:1561b6d61095 | 534 | } |
soulx | 36:1561b6d61095 | 535 | case UP_MARGIN: { |
soulx | 36:1561b6d61095 | 536 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 537 | memory.read(ADDRESS_UP_MARGIN,data_buff); |
soulx | 36:1561b6d61095 | 538 | Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin); |
soulx | 36:1561b6d61095 | 539 | com.sendUpMargin(MY_ID,UpMargin); |
soulx | 36:1561b6d61095 | 540 | break; |
soulx | 36:1561b6d61095 | 541 | } |
soulx | 36:1561b6d61095 | 542 | case LOW_MARGIN: { |
soulx | 36:1561b6d61095 | 543 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 544 | memory.read(ADDRESS_LOW_MARGIN,data_buff); |
soulx | 36:1561b6d61095 | 545 | Utilities::ConvertInt32ToUInt8Array(data_buff,LowMargin); |
soulx | 36:1561b6d61095 | 546 | com.sendLowMargin(MY_ID,LowMargin); |
soulx | 36:1561b6d61095 | 547 | break; |
soulx | 36:1561b6d61095 | 548 | } |
soulx | 36:1561b6d61095 | 549 | case PID_UPPER_MOTOR: { |
soulx | 36:1561b6d61095 | 550 | memory.read(ADDRESS_UPPER_KP,U_Kc); |
soulx | 36:1561b6d61095 | 551 | memory.read(ADDRESS_UPPER_KI,U_Ki_gain); |
soulx | 36:1561b6d61095 | 552 | memory.read(ADDRESS_UPPER_KD,U_Kd_gain); |
soulx | 36:1561b6d61095 | 553 | com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki_gain,U_Kd_gain); |
soulx | 36:1561b6d61095 | 554 | /* |
soulx | 36:1561b6d61095 | 555 | printf("After read Kp : %f\r\n",U_Kc); |
soulx | 36:1561b6d61095 | 556 | printf("After read Ki : %f\r\n",U_Ki_gain); |
soulx | 36:1561b6d61095 | 557 | printf("After read Kd : %f\r\n",U_Kd_gain); |
soulx | 36:1561b6d61095 | 558 | */ |
soulx | 36:1561b6d61095 | 559 | break; |
soulx | 36:1561b6d61095 | 560 | } |
soulx | 36:1561b6d61095 | 561 | case PID_LOWER_MOTOR: { |
soulx | 36:1561b6d61095 | 562 | memory.read(ADDRESS_LOWER_KP,L_Kc); |
soulx | 36:1561b6d61095 | 563 | memory.read(ADDRESS_LOWER_KI,L_Ki_gain); |
soulx | 36:1561b6d61095 | 564 | memory.read(ADDRESS_LOWER_KD,L_Kd_gain); |
soulx | 36:1561b6d61095 | 565 | com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki_gain,L_Kd_gain); |
soulx | 36:1561b6d61095 | 566 | /* |
soulx | 36:1561b6d61095 | 567 | printf("After read L_Kp : %f\r\n",L_Kc); |
soulx | 36:1561b6d61095 | 568 | printf("After read L_Ki : %f\r\n",L_Ki_gain); |
soulx | 36:1561b6d61095 | 569 | printf("After read L_Kd : %f\r\n",L_Kd_gain); |
soulx | 36:1561b6d61095 | 570 | */ |
soulx | 36:1561b6d61095 | 571 | break; |
soulx | 36:1561b6d61095 | 572 | } |
soulx | 36:1561b6d61095 | 573 | case HEIGHT: { |
soulx | 36:1561b6d61095 | 574 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 575 | memory.read(ADDRESS_HEIGHT,data_buff); |
soulx | 36:1561b6d61095 | 576 | Utilities::ConvertInt32ToUInt8Array(data_buff,Height); |
soulx | 36:1561b6d61095 | 577 | com.sendHeight(MY_ID,Height); |
soulx | 36:1561b6d61095 | 578 | break; |
soulx | 36:1561b6d61095 | 579 | } |
soulx | 36:1561b6d61095 | 580 | case WHEELPOS: { |
soulx | 36:1561b6d61095 | 581 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 582 | memory.read(ADDRESS_WHEELPOS,data_buff); |
soulx | 36:1561b6d61095 | 583 | Utilities::ConvertInt32ToUInt8Array(data_buff,Wheelpos); |
soulx | 36:1561b6d61095 | 584 | com.sendWheelPos(MY_ID,Wheelpos); |
soulx | 36:1561b6d61095 | 585 | break; |
soulx | 36:1561b6d61095 | 586 | } |
soulx | 36:1561b6d61095 | 587 | case MAG_DATA: { |
soulx | 36:1561b6d61095 | 588 | uint8_t x_max_array[4]; |
soulx | 36:1561b6d61095 | 589 | uint8_t x_min_array[4]; |
soulx | 36:1561b6d61095 | 590 | uint8_t y_max_array[4]; |
soulx | 36:1561b6d61095 | 591 | uint8_t y_min_array[4]; |
soulx | 36:1561b6d61095 | 592 | uint8_t z_max_array[4]; |
soulx | 36:1561b6d61095 | 593 | uint8_t z_min_array[4]; |
soulx | 36:1561b6d61095 | 594 | int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer; |
soulx | 36:1561b6d61095 | 595 | memory.read(ADDRESS_MAG_DATA,x_max_buffer); |
soulx | 36:1561b6d61095 | 596 | memory.read(ADDRESS_MAG_DATA+4,x_min_buffer); |
soulx | 36:1561b6d61095 | 597 | memory.read(ADDRESS_MAG_DATA+8,y_max_buffer); |
soulx | 36:1561b6d61095 | 598 | memory.read(ADDRESS_MAG_DATA+12,y_min_buffer); |
soulx | 36:1561b6d61095 | 599 | memory.read(ADDRESS_MAG_DATA+16,z_max_buffer); |
soulx | 36:1561b6d61095 | 600 | memory.read(ADDRESS_MAG_DATA+20,z_min_buffer); |
soulx | 36:1561b6d61095 | 601 | Utilities::ConvertInt32ToUInt8Array(x_max_buffer,x_max_array); |
soulx | 36:1561b6d61095 | 602 | Utilities::ConvertInt32ToUInt8Array(x_min_buffer,x_min_array); |
soulx | 36:1561b6d61095 | 603 | Utilities::ConvertInt32ToUInt8Array(y_max_buffer,y_max_array); |
soulx | 36:1561b6d61095 | 604 | Utilities::ConvertInt32ToUInt8Array(y_min_buffer,y_min_array); |
soulx | 36:1561b6d61095 | 605 | Utilities::ConvertInt32ToUInt8Array(z_max_buffer,z_max_array); |
soulx | 36:1561b6d61095 | 606 | Utilities::ConvertInt32ToUInt8Array(z_min_buffer,z_min_array); |
soulx | 36:1561b6d61095 | 607 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 608 | Mag[i]=x_max_array[i]; |
soulx | 36:1561b6d61095 | 609 | Mag[i+4]=x_min_array[i]; |
soulx | 36:1561b6d61095 | 610 | Mag[i+8]=y_max_array[i]; |
soulx | 36:1561b6d61095 | 611 | Mag[i+12]=y_min_array[i]; |
soulx | 36:1561b6d61095 | 612 | Mag[i+16]=z_max_array[i]; |
soulx | 36:1561b6d61095 | 613 | Mag[i+20]=z_min_array[i]; |
soulx | 36:1561b6d61095 | 614 | } |
soulx | 36:1561b6d61095 | 615 | com.sendMagData(MY_ID,Mag); |
soulx | 36:1561b6d61095 | 616 | break; |
soulx | 36:1561b6d61095 | 617 | } |
soulx | 36:1561b6d61095 | 618 | case OFFSET: { |
soulx | 36:1561b6d61095 | 619 | uint8_t y_offset_array[4]; |
soulx | 36:1561b6d61095 | 620 | uint8_t z_offset_array[4]; |
soulx | 36:1561b6d61095 | 621 | int32_t y_data_buffer,z_data_buffer; |
soulx | 36:1561b6d61095 | 622 | memory.read(ADDRESS_OFFSET,y_data_buffer); |
soulx | 36:1561b6d61095 | 623 | memory.read(ADDRESS_OFFSET+4,z_data_buffer); |
soulx | 36:1561b6d61095 | 624 | Utilities::ConvertInt32ToUInt8Array(y_data_buffer,y_offset_array); |
soulx | 36:1561b6d61095 | 625 | Utilities::ConvertInt32ToUInt8Array(z_data_buffer,z_offset_array); |
soulx | 36:1561b6d61095 | 626 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 627 | Offset[i]=y_offset_array[i]; |
soulx | 36:1561b6d61095 | 628 | Offset[i+4]=z_offset_array[i]; |
soulx | 36:1561b6d61095 | 629 | } |
soulx | 36:1561b6d61095 | 630 | com.sendOffset(MY_ID,Offset); |
soulx | 36:1561b6d61095 | 631 | break; |
soulx | 36:1561b6d61095 | 632 | } |
soulx | 36:1561b6d61095 | 633 | case BODY_WIDTH: { |
soulx | 36:1561b6d61095 | 634 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 635 | memory.read(ADDRESS_BODY_WIDTH,data_buff); |
soulx | 36:1561b6d61095 | 636 | Utilities::ConvertInt32ToUInt8Array(data_buff,Body_width); |
soulx | 36:1561b6d61095 | 637 | com.sendBodyWidth(MY_ID,Body_width); |
soulx | 36:1561b6d61095 | 638 | break; |
soulx | 36:1561b6d61095 | 639 | } |
soulx | 36:1561b6d61095 | 640 | case ANGLE_RANGE_UP: { |
soulx | 36:1561b6d61095 | 641 | uint8_t max_array[4]; |
soulx | 36:1561b6d61095 | 642 | uint8_t min_array[4]; |
soulx | 36:1561b6d61095 | 643 | int32_t max_data_buffer,min_data_buffer; |
soulx | 36:1561b6d61095 | 644 | memory.read(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); |
soulx | 36:1561b6d61095 | 645 | memory.read(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); |
soulx | 36:1561b6d61095 | 646 | Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array); |
soulx | 36:1561b6d61095 | 647 | Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array); |
soulx | 36:1561b6d61095 | 648 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 649 | Angle_Range_Up[i]=max_array[i]; |
soulx | 36:1561b6d61095 | 650 | Angle_Range_Up[i+4]=min_array[i]; |
soulx | 36:1561b6d61095 | 651 | } |
soulx | 36:1561b6d61095 | 652 | com.sendUpAngleRange(MY_ID,Angle_Range_Up); |
soulx | 36:1561b6d61095 | 653 | break; |
soulx | 36:1561b6d61095 | 654 | } |
soulx | 36:1561b6d61095 | 655 | case ANGLE_RANGE_LOW: { |
soulx | 36:1561b6d61095 | 656 | uint8_t max_array[4]; |
soulx | 36:1561b6d61095 | 657 | uint8_t min_array[4]; |
soulx | 36:1561b6d61095 | 658 | int32_t max_data_buffer,min_data_buffer; |
soulx | 36:1561b6d61095 | 659 | memory.read(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); |
soulx | 36:1561b6d61095 | 660 | memory.read(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); |
soulx | 36:1561b6d61095 | 661 | Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array); |
soulx | 36:1561b6d61095 | 662 | Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array); |
soulx | 36:1561b6d61095 | 663 | for(int i=0; i<4; i++) { |
soulx | 36:1561b6d61095 | 664 | Angle_Range_Low[i]=max_array[i]; |
soulx | 36:1561b6d61095 | 665 | Angle_Range_Low[i+4]=min_array[i]; |
soulx | 36:1561b6d61095 | 666 | } |
soulx | 36:1561b6d61095 | 667 | com.sendLowAngleRange(MY_ID,Angle_Range_Low); |
soulx | 36:1561b6d61095 | 668 | break; |
soulx | 36:1561b6d61095 | 669 | } |
soulx | 36:1561b6d61095 | 670 | case UP_LINK_LENGTH: { |
soulx | 36:1561b6d61095 | 671 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 672 | memory.read(ADDRESS_UP_LINK_LENGTH,data_buff); |
soulx | 36:1561b6d61095 | 673 | Utilities::ConvertInt32ToUInt8Array(data_buff,UpLinkLength); |
soulx | 36:1561b6d61095 | 674 | com.sendUpLinkLength(MY_ID,UpLinkLength); |
soulx | 36:1561b6d61095 | 675 | break; |
soulx | 36:1561b6d61095 | 676 | } |
soulx | 36:1561b6d61095 | 677 | case LOW_LINK_LENGTH: { |
soulx | 36:1561b6d61095 | 678 | int32_t data_buff; |
soulx | 36:1561b6d61095 | 679 | memory.read(ADDRESS_LOW_LINK_LENGTH,data_buff); |
soulx | 36:1561b6d61095 | 680 | Utilities::ConvertInt32ToUInt8Array(data_buff,LowLinkLength); |
soulx | 36:1561b6d61095 | 681 | com.sendLowLinkLength(MY_ID,LowLinkLength); |
soulx | 36:1561b6d61095 | 682 | break; |
soulx | 36:1561b6d61095 | 683 | } |
soulx | 36:1561b6d61095 | 684 | break; |
soulx | 36:1561b6d61095 | 685 | } |
soulx | 36:1561b6d61095 | 686 | } |
soulx | 36:1561b6d61095 | 687 | } |
soulx | 36:1561b6d61095 | 688 | } |
soulx | 36:1561b6d61095 | 689 | } |
soulx | 36:1561b6d61095 | 690 | |
soulx | 36:1561b6d61095 | 691 | |
soulx | 36:1561b6d61095 | 692 | /******************************************************/ |
soulx | 36:1561b6d61095 | 693 | void Start_Up() |
soulx | 36:1561b6d61095 | 694 | { |
soulx | 36:1561b6d61095 | 695 | // wait for reciever |
soulx | 36:1561b6d61095 | 696 | memory.read(ADDRESS_ID,MY_ID); |
soulx | 36:1561b6d61095 | 697 | memory.read(ADDRESS_UPPER_KP,U_Kc); |
soulx | 36:1561b6d61095 | 698 | memory.read(ADDRESS_UPPER_KI,U_Ti); |
soulx | 36:1561b6d61095 | 699 | memory.read(ADDRESS_UPPER_KD,U_Td); |
soulx | 36:1561b6d61095 | 700 | memory.read(ADDRESS_LOWER_KP,L_Kc); |
soulx | 36:1561b6d61095 | 701 | memory.read(ADDRESS_LOWER_KI,L_Ti); |
soulx | 36:1561b6d61095 | 702 | memory.read(ADDRESS_LOWER_KD,L_Td); |
soulx | 36:1561b6d61095 | 703 | |
soulx | 36:1561b6d61095 | 704 | } |
soulx | 36:1561b6d61095 | 705 | /*******************************************************/ |
soulx | 36:1561b6d61095 | 706 | void Rc() |
soulx | 36:1561b6d61095 | 707 | { |
soulx | 36:1561b6d61095 | 708 | myled =1; |
soulx | 36:1561b6d61095 | 709 | uint8_t data_array[30]; |
soulx | 36:1561b6d61095 | 710 | uint8_t id=0; |
soulx | 36:1561b6d61095 | 711 | uint8_t ins=0; |
soulx | 36:1561b6d61095 | 712 | uint8_t status=0xFF; |
soulx | 36:1561b6d61095 | 713 | |
soulx | 36:1561b6d61095 | 714 | |
soulx | 36:1561b6d61095 | 715 | |
soulx | 36:1561b6d61095 | 716 | status = com.ReceiveCommand(&id,data_array,&ins); |
soulx | 36:1561b6d61095 | 717 | //PC.printf("status = 0x%02x\n\r",status); |
soulx | 36:1561b6d61095 | 718 | if(status == ANDANTE_ERRBIT_NONE) { |
soulx | 36:1561b6d61095 | 719 | CmdCheck((int16_t)id,data_array,ins); |
soulx | 36:1561b6d61095 | 720 | } |
soulx | 36:1561b6d61095 | 721 | |
soulx | 36:1561b6d61095 | 722 | } |
soulx | 36:1561b6d61095 | 723 | /*******************************************************/ |
soulx | 36:1561b6d61095 | 724 | int main() |
soulx | 36:1561b6d61095 | 725 | { |
soulx | 36:1561b6d61095 | 726 | PC.baud(115200); |
soulx | 36:1561b6d61095 | 727 | /* |
soulx | 36:1561b6d61095 | 728 | while(1) |
soulx | 36:1561b6d61095 | 729 | { |
soulx | 36:1561b6d61095 | 730 | Read_Encoder(EncoderA); |
soulx | 36:1561b6d61095 | 731 | Upper_Position = Get_EnValue(data); |
soulx | 36:1561b6d61095 | 732 | Read_Encoder(EncoderB); |
soulx | 36:1561b6d61095 | 733 | Lower_Position = Get_EnValue(data); |
soulx | 36:1561b6d61095 | 734 | PC.printf("Upper Position : %f\n",Upper_Position); |
soulx | 36:1561b6d61095 | 735 | PC.printf("Lower_Position : %f\n",Lower_Position); |
soulx | 36:1561b6d61095 | 736 | wait(0.5); |
soulx | 36:1561b6d61095 | 737 | } |
soulx | 36:1561b6d61095 | 738 | */ |
soulx | 36:1561b6d61095 | 739 | |
soulx | 36:1561b6d61095 | 740 | |
soulx | 36:1561b6d61095 | 741 | //Recieve.attach(&Rc,0.025); |
soulx | 36:1561b6d61095 | 742 | //Start_Up(); |
soulx | 36:1561b6d61095 | 743 | |
soulx | 36:1561b6d61095 | 744 | SET_UpperPID(); |
soulx | 36:1561b6d61095 | 745 | SET_LowerPID(); |
soulx | 36:1561b6d61095 | 746 | |
soulx | 36:1561b6d61095 | 747 | printf("BEAR MOTION\n\r"); |
soulx | 36:1561b6d61095 | 748 | while(1) { |
soulx | 36:1561b6d61095 | 749 | myled =0; |
soulx | 36:1561b6d61095 | 750 | //wait_ms(10); |
soulx | 36:1561b6d61095 | 751 | ///////////////////////////////////////////////////// start |
soulx | 36:1561b6d61095 | 752 | //Set Set_point |
soulx | 36:1561b6d61095 | 753 | Up_PID.setSetPoint(Upper_SetPoint); |
soulx | 36:1561b6d61095 | 754 | Low_PID.setSetPoint(Lower_SetPoint); |
soulx | 36:1561b6d61095 | 755 | |
soulx | 36:1561b6d61095 | 756 | Read_Encoder(EncoderB); |
soulx | 36:1561b6d61095 | 757 | Lower_Position = Get_EnValue(data); |
soulx | 36:1561b6d61095 | 758 | PC.printf("position = %2f\n",Lower_Position); |
soulx | 36:1561b6d61095 | 759 | //Control Motor |
soulx | 36:1561b6d61095 | 760 | //Move_Upper(); |
soulx | 36:1561b6d61095 | 761 | //Move_Lower(); |
soulx | 36:1561b6d61095 | 762 | ///////////////////////////////////////////////////// stop =306us |
soulx | 36:1561b6d61095 | 763 | //uint8_t aaa[1]={10}; |
soulx | 36:1561b6d61095 | 764 | //com.sendBodyWidth(0x01,aaa); |
soulx | 36:1561b6d61095 | 765 | Rc(); |
soulx | 36:1561b6d61095 | 766 | //wait_ms(1); |
soulx | 36:1561b6d61095 | 767 | } |
soulx | 36:1561b6d61095 | 768 | } |
soulx | 36:1561b6d61095 | 769 | |
soulx | 36:1561b6d61095 | 770 | |
soulx | 36:1561b6d61095 | 771 | |
soulx | 36:1561b6d61095 | 772 | |
soulx | 36:1561b6d61095 | 773 | |
soulx | 36:1561b6d61095 | 774 |