初定版本
Dependencies: mbed Hobbyking_Cheetah FastPWM3
Revision 55:d57335792cb7, committed 2022-05-10
- Comitter:
- qiudehua
- Date:
- Tue May 10 02:10:43 2022 +0000
- Parent:
- 54:59575833d16f
- Commit message:
- GT_MOTOR_24NM_V03
Changed in this revision
diff -r 59575833d16f -r d57335792cb7 CAN/CAN_com.cpp --- a/CAN/CAN_com.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/CAN/CAN_com.cpp Tue May 10 02:10:43 2022 +0000 @@ -12,6 +12,20 @@ #define T_MIN -18.0f #define T_MAX 18.0f + /* + //为保证设置参数的最小分辨率,可启用下列参数 + //注意:必须保证上、下转换时,MIN/MAX的一致,否则数据会出错!!! + #define P_MIN -150f + #define P_MAX 150f + #define V_MIN -270.0f + #define V_MAX 270.0f + #define KP_MIN 0.0f + #define KP_MAX 50.0f + #define KD_MIN 0.0f + #define KD_MAX 5.0f + #define T_MIN -5.0f + #define T_MAX 5.0f + */ /// CAN Reply Packet Structure /// /// 16 bit position, between -4*pi and 4*pi @@ -61,9 +75,13 @@ controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); - controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); + controller->v_des /= GR; // plan A&B&C, modify by 20210630 + controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); + controller->kp *= GR; // plan A&B&C, modify by 20210630 controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); + //controller->kd /= GR; // plan B&C, modify by 20210630 controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); + controller->t_ff *= GR; // plan A&C, modify by 20210630 //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 d57335792cb7 CAN/CAN_com.h --- a/CAN/CAN_com.h Thu Aug 08 17:39:43 2019 +0000 +++ b/CAN/CAN_com.h Tue May 10 02:10:43 2022 +0000 @@ -5,6 +5,7 @@ #include "user_config.h" #include "mbed.h" #include "../math_ops.h" +#include "motor_config.h" void pack_reply(CANMessage *msg, float p, float v, float t); void unpack_cmd(CANMessage msg, ControllerStruct * controller);
diff -r 59575833d16f -r d57335792cb7 Calibration/calibration.cpp --- a/Calibration/calibration.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/Calibration/calibration.cpp Tue May 10 02:10:43 2022 +0000 @@ -232,7 +232,8 @@ ps->WriteLUT(lut); // write lookup table to position sensor object //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... - memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array + //memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array + memcpy(&ENCODER_LUT, lut, 512); // copy the lookup table to the flash array // modified on 20210615 printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); if (!prefs->ready()) prefs->open();
diff -r 59575833d16f -r d57335792cb7 Config/motor_config.h --- a/Config/motor_config.h Thu Aug 08 17:39:43 2019 +0000 +++ b/Config/motor_config.h Tue May 10 02:10:43 2022 +0000 @@ -4,10 +4,13 @@ #define R_PHASE 0.13f //Ohms #define L_D 0.00002f //Henries #define L_Q 0.00002f //Henries -#define KT .08f //N-m per peak phase amp, = WB*NPP*3/2 +//#define KT .08f //N-m per peak phase amp, = WB*NPP*3/2 +//#define KT 0.07875f //N-m per peak phase amp, = WB*NPP*3/2 = 0.0025*21*3/2 = 0.07875 +#define KT 0.2f //N-m per peak phase amp, = WB*NPP*3/2 = 0.0025*21*3/2 = 0.07875 #define NPP 21 //Number of pole pairs #define GR 6.0f //Gear ratio -#define KT_OUT 0.45f //KT*GR +//#define KT_OUT 0.45f //KT*GR +#define KT_OUT 1.2f //KT*GR #define WB 0.0025f //Flux linkage, Webers. #define R_TH 1.25f //Kelvin per watt #define INV_M_TH 0.03125f //Kelvin per joule
diff -r 59575833d16f -r d57335792cb7 DRV8323/DRV.cpp --- a/DRV8323/DRV.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/DRV8323/DRV.cpp Tue May 10 02:10:43 2022 +0000 @@ -68,6 +68,7 @@ void DRV832x::print_faults(void) { + wait_us(10); uint16_t val1 = read_FSR1(); wait_us(10); uint16_t val2 = read_FSR2();
diff -r 59575833d16f -r d57335792cb7 Hobbyking_Cheetah_V02.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Hobbyking_Cheetah_V02.lib Tue May 10 02:10:43 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah/#59575833d16f
diff -r 59575833d16f -r d57335792cb7 PositionSensor/PositionSensor.cpp --- a/PositionSensor/PositionSensor.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/PositionSensor/PositionSensor.cpp Tue May 10 02:10:43 2022 +0000 @@ -38,10 +38,12 @@ int angle = raw + off_interp; // Correct for nonlinearity with lookup table from calibration if(angle - old_counts > _CPR/2){ rotations -= 1; - } + //if(rotations< -14) rotations = 14.5f; // modify by 20210706 + } else if (angle - old_counts < -_CPR/2){ - rotations += 1; - } + rotations += 1; + //if(rotations> 14) rotations = -15.5f; // modify by 20210706 + } old_counts = angle; oldModPosition = modPosition; @@ -65,16 +67,42 @@ } else{ vel = (modPosition-oldModPosition)/dt; - } + } - int n = 40; + /* + int n = 21; float sum = vel; for (int i = 1; i < (n); i++){ velVec[n - i] = velVec[n-i-1]; sum += velVec[n-i]; - } + } velVec[0] = vel; - MechVelocity = sum/((float)n); + MechVelocity = sum/((float)n); // modify by 20210706 + + float sumLow = MechVelocity; + for (int i = 1; i < (n); i++){ + velVecLow[n - i] = velVecLow[n-i-1]; + sumLow += velVecLow[n-i]; + } + velVecLow[0] = MechVelocity; + MechVelocityLow = sumLow/((float)n); // modify by 20210706 + */ + + //********************************************************* + int n = 168; + float sum = vel; + float sumLow = vel; + for (int i = 1; i < (n); i++){ + velVec[n - i] = velVec[n-i-1]; + sumLow += velVec[n-i]; + if(i > 147) sum +=velVec[n-i]; + } + + velVec[0] = vel; + MechVelocity = sum/21; // modify by 20210706 + MechVelocityLow = sumLow/((float)n); // modify by 20210706 + //********************************************************* + ElecVelocity = MechVelocity*_ppairs; ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity; } @@ -101,6 +129,7 @@ float PositionSensorAM5147::GetMechVelocity(){ return MechVelocity; + //return MechVelocityLow; } void PositionSensorAM5147::ZeroPosition(){
diff -r 59575833d16f -r d57335792cb7 PositionSensor/PositionSensor.h --- a/PositionSensor/PositionSensor.h Thu Aug 08 17:39:43 2019 +0000 +++ b/PositionSensor/PositionSensor.h Tue May 10 02:10:43 2022 +0000 @@ -37,7 +37,8 @@ virtual void ZeroEncoderCountDown(void); int _CPR, flag, rotations, _ppairs, raw; //int state; - float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40]; + //float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40]; + float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[168]; int offset_lut[128]; }; @@ -57,7 +58,9 @@ virtual int GetCPR(void); virtual void WriteLUT(int new_lut[128]); private: - float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt; + //float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt; + float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[168], MechVelocity, ElecVelocity, ElecVelocityFilt; + float MechVelocityLow,velVecLow[168]; // add by 20210706 int raw, _CPR, rotations, old_counts, _ppairs; SPI *spi; DigitalOut *cs;
diff -r 59575833d16f -r d57335792cb7 main.cpp --- a/main.cpp Thu Aug 08 17:39:43 2019 +0000 +++ b/main.cpp Tue May 10 02:10:43 2022 +0000 @@ -10,7 +10,7 @@ #define SETUP_MODE 4 #define ENCODER_MODE 5 -#define VERSION_NUM "1.9" +#define VERSION_NUM "0.3" float __float_reg[64]; // Floats stored in flash @@ -54,7 +54,7 @@ PositionSensorAM5147 spi(16384, 0.0, NPP); -volatile int count = 0; +volatile int counts = 0; volatile int state = REST_MODE; volatile int state_change; @@ -75,11 +75,17 @@ } 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]==0xFF))){ + //Null instruction, which can be used to return data } else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); } - pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); + //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); + //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT); // modify by 20210617 + //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT); // 正式使用 plan A&C, modify by 20210630 + pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT); // test v?, modify by 20210706 can.write(txMsg); } @@ -166,7 +172,6 @@ /// This runs at 40 kHz, regardless of of the mode the controller is in /// extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { - ///Sample current always /// ADC1->CR2 |= 0x40000000; // Begin sample and conversion //volatile int delay; @@ -177,8 +182,11 @@ controller.adc1_raw = ADC1->DR; controller.adc3_raw = ADC3->DR; controller.theta_elec = spi.GetElecPosition(); - controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); - controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); + //controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); + controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); + controller.theta_mech = (1.0f)*spi.GetMechPosition(); // modified on 20210617 + //controller.dtheta_mech = (1.0f)*spi.GetMechVelocity(); // modified on 20210618 + controller.dtheta_elec = spi.GetElecVelocity(); controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement /// @@ -199,8 +207,11 @@ case MOTOR_MODE: // Run torque control if(state_change){ + enter_torque_mode(); - count = 0; + + counts = 0; //20210222 + } else{ /* @@ -224,9 +235,8 @@ torque_control(&controller); commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop - controller.timeout++; - count++; + counts ++; } break; @@ -238,7 +248,7 @@ case ENCODER_MODE: print_encoder(); break; - } + } } TIM1->SR = 0x0; // reset the status register } @@ -269,7 +279,7 @@ break; case 'm': state = MOTOR_MODE; - state_change = 1; + state_change = 1; break; case 'e': state = ENCODER_MODE; @@ -360,11 +370,23 @@ } int main() { + //************************* + //uint32_t btr; + //CAN_TypeDef *can = obj->CanHandle.Instance; + /* + hcan->Instance->BTR = (uint32_t)((uint32_t)hcan->Init.Mode) | \ + ((uint32_t)hcan->Init.SJW) | \ + ((uint32_t)hcan->Init.BS1) | \ + ((uint32_t)hcan->Init.BS2) | \ + ((uint32_t)hcan->Init.Prescaler - 1U); + */ + + controller.v_bus = V_BUS; controller.mode = 0; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO wait(.1); - + gpio.enable->write(1); wait_us(100); drv.calibrate(); @@ -374,7 +396,7 @@ drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); wait_us(100); drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88); - + //drv.enable_gd(); zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset drv.disable_gd(); @@ -387,6 +409,7 @@ TIM1->CCR1 = 0x708*(1.0f); gpio.enable->write(0); */ + reset_foc(&controller); // Reset current controller reset_observer(&observer); // Reset observer TIM1->CR1 ^= TIM_CR1_UDIS; @@ -411,7 +434,7 @@ if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} - if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;} + if(isnan(I_MAX) || I_MAX ==-1){I_MAX=30;} if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;} if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} @@ -424,8 +447,9 @@ init_controller_params(&controller); pc.baud(921600); // set serial baud rate + pc.attach(&serial_interrupt); // attach serial interrupt wait(.01); - pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); + pc.printf("\n\r\n\r GTGJ MOTOR\n\r\n\r"); wait(.01); printf("\n\r Debug Info:\n\r"); printf(" Firmware Version: %s\n\r", VERSION_NUM); @@ -435,8 +459,7 @@ printf(" CAN ID: %d\n\r", CAN_ID); - - +//********* //printf(" %d\n\r", drv.read_register(DCR)); //wait_us(100); //printf(" %d\n\r", drv.read_register(CSACR)); @@ -444,25 +467,45 @@ //printf(" %d\n\r", drv.read_register(OCPCR)); //drv.disable_gd(); - pc.attach(&serial_interrupt); // attach serial interrupt +// pc.attach(&serial_interrupt); // attach serial interrupt state_change = 1; - int counter = 0; + //********************************************************************************************** + /* + CAN_HandleTypeDef* hcan; + //3M --> brp=2,TS1=0,TS2=2,SJW=1 + //5M --> brp=2,TS1=0,TS2=0,SJW=1 + btr = ((2 << CAN_BTR_TS2_Pos) & CAN_BTR_TS2) | + ((0 << CAN_BTR_TS1_Pos) & CAN_BTR_TS1) | + ((1 << CAN_BTR_SJW_Pos) & CAN_BTR_SJW) | + ((2 << CAN_BTR_BRP_Pos) & CAN_BTR_BRP); + hcan->Instance->BTR &= ~(CAN_BTR_TS2 | CAN_BTR_TS1 | CAN_BTR_SJW | CAN_BTR_BRP); + hcan->Instance->BTR |= btr; + */ + //********************************************************************************************** + + //CAN_TypeDef *can = obj->CanHandle.Instance; + //state_change = can->MSR; + +// int counter = 0; while(1) { drv.print_faults(); wait(.1); - //printf("%.4f\n\r", controller.v_bus); - /* + //printf("%.4f\n\r", controller.v_bus); + //* if(state == MOTOR_MODE) { //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); - //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec); - //printf("%.3f\n\r", controller.dtheta_mech); - wait(.002); + //printf("%.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.dtheta_elec); + //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.i_a, controller.i_b, controller.i_c,controller.theta_elec,controller.i_d, controller.i_q,controller.i_d_filt, controller.i_q_filt); + //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.theta_elec,controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT/GR); + //printf("%.3f\n\r", controller.dtheta_mech); + //printf("%d %.4f %.4f\n\r", spi.GetRawPosition(), spi.GetMechPositionFixed(),spi.GetMechPosition()); +//printf("%d %d %d\n\r", can->MSR, can->TSR,can->ESR); + wait(.005); } - */ - + //*/ } }
diff -r 59575833d16f -r d57335792cb7 mbed-dev.lib --- a/mbed-dev.lib Thu Aug 08 17:39:43 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a
diff -r 59575833d16f -r d57335792cb7 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue May 10 02:10:43 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file