Test the set param mode
Revision 55:6ab83cbbbca7, committed 2021-03-26
- Comitter:
- eatinghsieh
- Date:
- Fri Mar 26 05:28:00 2021 +0000
- Parent:
- 54:59575833d16f
- Commit message:
- Test set parameter mode
Changed in this revision
diff -r 59575833d16f -r 6ab83cbbbca7 CAN/CAN_com.cpp --- a/CAN/CAN_com.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/CAN/CAN_com.cpp Fri Mar 26 05:28:00 2021 +0000 @@ -12,6 +12,20 @@ #define T_MIN -18.0f #define T_MAX 18.0f + #define kp_L_MAX 125.0f + #define kp_L_MIN 0.0f + #define kp_M1_MAX 250.0f + #define kp_M1_MIN 126.0f + #define kp_M2_MAX 375.0f + #define kp_M2_MIN 251.0f + #define kp_H_MAX 500.0f + #define kp_H_MIN 376.0f + #define range_L_MAX 256.0f + #define range_L_MIN 256.0f + #define range_M_MAX 256.0f + #define range_M_MIN 256.0f + #define range_H_MAX 256.0f + #define range_H_MIN 256.0f /// CAN Reply Packet Structure /// /// 16 bit position, between -4*pi and 4*pi @@ -35,7 +49,60 @@ msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8); msg->data[5] = t_int&0xFF; } + +/// CAN Command Packet Structure /// +/// 16 bit position command, between -4*pi and 4*pi +/// 12 bit velocity command, between -30 and + 30 rad/s +/// 12 bit kp, between 0 and 500 N-m/rad +/// 12 bit kd, between 0 and 100 N-m*s/rad +/// 12 bit feed forward torque, between -18 and 18 N-m +/// CAN Packet is 8 8-bit words +/// Formatted as follows. For each quantity, bit 0 is LSB +/// 0: [position[15-8]] +/// 1: [position[7-0]] +/// 2: [velocity[11-4]] +/// 3: [velocity[3-0], kp[11-8]] +/// 4: [kp[7-0]] +/// 5: [kd[11-4]] +/// 6: [kd[3-0], torque[11-8]] +/// 7: [torque[7-0]] +void unpack_cmd(CANMessage msg, ControllerStruct * controller){ + int p_int = (msg.data[0]<<16)|(msg.data[1]<<8)|msg.data[2]; + int v_int = (msg.data[3]<<12)|(msg.data[4]<<4)|(msg.data[5]>>4); + int t_int = ((msg.data[5]&0xF)<<16)|(msg.data[6]<<8)|msg.data[7]; + + controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 24); + controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 20); + controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 20); + //printf("Received "); + //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); + //printf("\n\r"); + } + +void unpack_param(CANMessage msg, ControllerStruct * controller){ + int kp_L_int = msg.data[0]; + int kp_M1_int = msg.data[1]; + int kp_M2_int = msg.data[2]; + int kp_H_int = msg.data[3]; + int kd_int = msg.data[4]; + int range_L_int = msg.data[5]; + int range_M_int = msg.data[6]; + int range_H_int = msg.data[7]; + + controller->kp_L = uint_to_float(kp_L_int, kp_L_MIN, kp_L_MAX, 8); + controller->kp_M1 = uint_to_float(kp_M1_int, kp_M1_MIN, kp_M1_MAX, 8); + controller->kp_M2 = uint_to_float(kp_M2_int, kp_M2_MIN, kp_M2_MAX, 8); + controller->kp_H = uint_to_float(kp_H_int, kp_H_MIN, kp_H_MAX, 8); + controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 8); + controller->range_L = uint_to_float(range_L_int, range_L_MIN, range_L_MAX, 8); + controller->range_M = uint_to_float(range_M_int, range_M_MIN, range_M_MAX, 8); + controller->range_H = uint_to_float(range_H_int, range_H_MIN, range_H_MAX, 8); + //printf("Received "); + //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); + //printf("\n\r"); + } +/* /// CAN Command Packet Structure /// /// 16 bit position command, between -4*pi and 4*pi /// 12 bit velocity command, between -30 and + 30 rad/s @@ -67,5 +134,5 @@ //printf("Received "); //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); //printf("\n\r"); - } + }*/
diff -r 59575833d16f -r 6ab83cbbbca7 CAN/CAN_com.h --- a/CAN/CAN_com.h Thu Aug 08 17:39:43 2019 +0000 +++ b/CAN/CAN_com.h Fri Mar 26 05:28:00 2021 +0000 @@ -8,6 +8,7 @@ void pack_reply(CANMessage *msg, float p, float v, float t); void unpack_cmd(CANMessage msg, ControllerStruct * controller); +void unpack_param(CANMessage msg, ControllerStruct * controller); #endif \ No newline at end of file
diff -r 59575833d16f -r 6ab83cbbbca7 FOC/foc.cpp --- a/FOC/foc.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/FOC/foc.cpp Fri Mar 26 05:28:00 2021 +0000 @@ -51,7 +51,7 @@ void linearize_dtc(float *dtc){ /// linearizes the output of the inverter, which is not linear for small duty cycles /// - float sgn = 1.0f-(2.0f*(dtc<0)); + float sgn = 1.0f-(2.0f*(*dtc<0)); if(abs(*dtc) >= .01f){ *dtc = *dtc*.986f+.014f*sgn; } @@ -218,11 +218,32 @@ } +void torque_control(ControllerStruct *controller){ + float error = controller->p_des - controller->theta_mech; + float kp; + if( error <= controller->range_L ) + kp = controller->kp_H; + else if ( error > controller->range_L && error <= controller->range_M ) + kp = controller->kp_M2; + else if ( error > controller->range_M && error <= controller->range_H ) + kp = controller->kp_M1; + else + kp = controller->kp_L; + + float torque_ref = kp*error + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech); + //float torque_ref = -.1*(controller->p_des - controller->theta_mech); + controller->i_q_ref = torque_ref/KT_OUT; + controller->i_d_ref = 0.0f; + } + + +/* void torque_control(ControllerStruct *controller){ float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech); //float torque_ref = -.1*(controller->p_des - controller->theta_mech); controller->i_q_ref = torque_ref/KT_OUT; controller->i_d_ref = 0.0f; } +*/ \ No newline at end of file
diff -r 59575833d16f -r 6ab83cbbbca7 main.cpp --- a/main.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/main.cpp Fri Mar 26 05:28:00 2021 +0000 @@ -9,6 +9,7 @@ #define MOTOR_MODE 2 #define SETUP_MODE 4 #define ENCODER_MODE 5 +#define SETPARAM_MODE 6 #define VERSION_NUM "1.9" @@ -76,9 +77,16 @@ else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){ spi.ZeroPosition(); } + else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFB))){ + state = SETPARAM_MODE; + state_change = 1; + } else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); } + else if(state == SETPARAM_MODE){ + unpack_param(rxMsg, &controller); + } pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); can.write(txMsg); } @@ -217,7 +225,10 @@ if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; - controller.kp = 0; + controller.kp_H = 0; + controller.kp_L = 0; + controller.kp_M1 = 0; + controller.kp_M2 = 0; controller.kd = 0; controller.t_ff = 0; } @@ -402,6 +413,7 @@ prefs.load(); // Read flash can.filter(CAN_ID , 0xFFF, CANStandard, 0); + //can.filter(1 , 0xFFF, CANStandard, 1); // Example, show how to add can dataframe filter txMsg.id = CAN_MASTER; txMsg.len = 6;
diff -r 59575833d16f -r 6ab83cbbbca7 mbed-dev.lib --- a/mbed-dev.lib Thu Aug 08 17:39:43 2019 +0000 +++ b/mbed-dev.lib Fri Mar 26 05:28:00 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a +https://github.com/aiRobots-athome/mbed-dev.git/#38c17c2e85b8f9cc9015267d0036f1580cbe5f8f
diff -r 59575833d16f -r 6ab83cbbbca7 structs.h --- a/structs.h Thu Aug 08 17:39:43 2019 +0000 +++ b/structs.h Fri Mar 26 05:28:00 2021 +0000 @@ -33,7 +33,10 @@ int timeout; // Watchdog counter int mode; int ovp_flag; // Over-voltage flag - float p_des, v_des, kp, kd, t_ff; // Desired position, velocity, gians, torque + // float p_des, v_des, kp, kd, t_ff; // Desired position, velocity, gians, torque + float p_des, v_des, t_ff; // Desired position, velocity, torque + float kp_L, kp_M1, kp_M2, kp_H, kd ; // kp, kd + float range_L, range_M, range_H; // The range of error float v_ref, fw_int; // output voltage magnitude, field-weakening integral float cogging[128]; } ControllerStruct;