modified 0511
Dependencies: mbed-dev FastPWM3
main.cpp
- Committer:
- WinnieLiu
- Date:
- 2022-03-22
- Revision:
- 64:fd695fb9865b
- Parent:
- 63:5946297ba2b0
- Child:
- 65:cb3497b549e9
File content as of revision 64:fd695fb9865b:
/// high-bandwidth 3-phase motor control, for robots /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others /// Hardware documentation can be found at build-its.blogspot.com /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling /// Version for the TI DRV8323 Everything Chip #define REST_MODE 0 #define MOTOR_MODE 1 #define HALL_CALIBRATE 2 //hall sensor calibration #define SETUP_MODE 3 #define CALIBRATION_MODE 4 #define ENCODER_MODE 5 #define VERSION_NUM 7 //float __float_reg[64]; // Floats stored in flash //float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd) //float __float_reg[68]; // add calibrate offset float __float_reg[69]; // add calibrate speed //float __float_reg[66]; int __int_reg[257]; //新增calibrate旋轉方向(+1: 逆時針旋轉、-1: 順時針旋轉) //int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table #include "mbed.h" #include "PositionSensor.h" #include "structs.h" #include "foc.h" #include "calibration.h" #include "hw_setup.h" #include "math_ops.h" #include "current_controller_config.h" #include "hw_config.h" #include "motor_config.h" #include "stm32f4xx_flash.h" #include "FlashWriter.h" #include "user_config.h" #include "PreferenceWriter.h" #include "CAN_com.h" #include "DRV.h" //DigitalOut *CAN_DEBUG = new DigitalOut(PB_15); //DigitalOut *GPIO_PID_DEBUG = new DigitalOut(PC_6); DigitalIn *HALL_IO = new DigitalIn(PC_6); PreferenceWriter prefs(6); GPIOStruct gpio; ControllerStruct controller; ObserverStruct observer; COMStruct com; Serial pc(PA_2, PA_3); CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name CANMessage rxMsg; CANMessage txMsg; SPI drv_spi(PA_7, PA_6, PA_5); DigitalOut drv_cs(PA_4); //DigitalOut drv_en_gate(PA_11); DRV832x drv(&drv_spi, &drv_cs); PositionSensorAM5147 spi(16384, 0.0, NPP); volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; void ChangeParameter(CANMessage msg) ; //bool complete_changepara = false; /*Hall sensor calibration*/ volatile int hall_input = 1; volatile int hall_preinput = 1; volatile float cal_pcmd = 0; volatile float calibrate_speed = 0.25; // rad/s volatile float hall_presentpos = 0; //calibrate之前encoder的位置 volatile float hall_in_pos = 0; //讀到1->0的位置(磁鐵進入hall sensor範圍) volatile float hall_out_pos = 0; //讀到0->1的位置(磁鐵出hall sensor範圍) volatile float hall_mid_pos = 0; volatile float calibrate_offset = 0; //rad volatile int calibrate_count = 0; volatile int calibrate_state = 0; // void onMsgReceived() { //static int can_state = 0; //msgAvailable = true; //printf("%d\n\r", rxMsg.id); //CAN_DEBUG->write(1); can.read(rxMsg); if((rxMsg.id == CAN_ID)){ controller.timeout = 0; //printf("%X\n\r",0x55); 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]==0xFC))){ //printf("can motor\n\r"); state = MOTOR_MODE; state_change = 1; } 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]==0xFD))){ state = REST_MODE; state_change = 1; gpio.led->write(0); } 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(); controller.p_des = 0; } // else if(state == REST_MODE && rxMsg.data[0]==0xFE){ // printf("Change Parameters\n\r"); // ChangeParameter(rxMsg); // complete_changepara = true; // } 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]==0xFA))){ //printf("Hall Calibration\n\r"); calibrate_count = 0; calibrate_state = 1; // calibrating //step1:讀目前位置 /*----- convert theta_mech to 0~359.9999deg -----*/ hall_presentpos = controller.theta_mech; cal_pcmd = controller.theta_mech; float _f_cal_round; modf(cal_pcmd/(2*PI),&_f_cal_round); cal_pcmd = cal_pcmd - _f_cal_round*2*PI; if(cal_pcmd < 0) cal_pcmd = cal_pcmd + 2*PI; state = HALL_CALIBRATE ; state_change = 1; } else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); } pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state); can.write(txMsg); //can_state = can.write(txMsg); //CAN_DEBUG->write(0); //CAN_DEBUG->write(can_state); } } //Use CAN Bus to change parameters //void ChangeParameter(CANMessage msg){ // int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4); // int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; // int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); // // CAN_ID = msg.data[1]; // MOTOR_KP = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); // MOTOR_KI = uint_to_float(ki_int, KI_MIN, KI_MAX, 12); // MOTOR_KD = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); // // // save to flash // if (!prefs.ready()) prefs.open(); // prefs.flush(); // Write new prefs to flash // prefs.close(); // prefs.load(); // /*----- change new pid controller parameter -----*/ // controller.kp = MOTOR_KP; // controller.ki = MOTOR_KI; // controller.kd = MOTOR_KD; //} void enter_menu_state(void){ drv.disable_gd(); gpio.enable->write(0); printf("\n\r\n\r\n\r"); printf(" Commands:\n\r"); wait_us(10); printf(" m - Motor Mode\n\r"); wait_us(10); printf(" c - Calibrate Encoder\n\r"); wait_us(10); printf(" s - Setup\n\r"); wait_us(10); printf(" e - Display Encoder\n\r"); wait_us(10); printf(" z - Set Zero Position\n\r"); wait_us(10); printf(" esc - Exit to Menu\n\r"); wait_us(10); state_change = 0; gpio.led->write(0); } void enter_setup_state(void){ printf("\n\r\n\r Configuration Options \n\r\n\n"); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "o", "CAN ID", "0", "127", CAN_ID); wait_us(10); printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX); wait_us(10); printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); wait_us(10); printf(" %-4s %-31s %-5s %-6s %d\n\r", "a", "Calibration Direction", "-1", "1", CALIBRATE_DIR); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "e", "Calibration offset", "0.0", "143.0", CALIBRATE_OFFSET); wait_us(10); printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Calibration Speed", "0.0", "10.0", CALIBRATE_SPEED); wait_us(10); printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP); wait_us(10); printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI); wait_us(10); printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD); wait_us(10); printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); wait_us(10); state_change = 0; } void enter_torque_mode(void){ drv.enable_gd(); gpio.enable->write(1); controller.ovp_flag = 0; reset_foc(&controller); // Tesets integrators, and other control loop parameters wait(.001); controller.i_d_ref = 0; controller.i_q_ref = 0; // Current Setpoints gpio.led->write(1); // Turn on status LED state_change = 0; printf("\n\r Entering Motor Mode \n\r"); } void calibrate(void){ drv.enable_gd(); gpio.enable->write(1); gpio.led->write(1); // Turn on status LED order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure gpio.led->write(0);; // Turn off status LED wait(.2); printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); drv.disable_gd(); gpio.enable->write(0); state_change = 0; } void print_encoder(void){ printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //printf("%d\n\r", spi.GetRawPosition()); wait(.001); } /// Current Sampling Interrupt /// /// 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 ) { //GPIO_PID_DEBUG->write(1); ///Sample current always /// ADC1->CR2 |= 0x40000000; // Begin sample and conversion //volatile int delay; //for (delay = 0; delay < 55; delay++); spi.Sample(DT); // sample position sensor controller.adc2_raw = ADC2->DR; // Read ADC Data Registers 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.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 // Calibration function if(calibrate_state == 0 || calibrate_state >= 2 ) ; else{ // read hall sensor hall_input = HALL_IO->read() ; // calculate new pos if((CALIBRATE_DIR == 1 && controller.theta_mech >= hall_presentpos + 2*PI) || (CALIBRATE_DIR == -1 && controller.theta_mech <= hall_presentpos - 2*PI)){ calibrate_state = 3 ; state = REST_MODE ; state_change = 1 ; } else{ // step3: 馬達正反轉讀hall sensor(1: 沒有感應到磁鐵,0:有感應到磁鐵) // 記住1->0瞬間的位置(in_pos),繼續旋轉 // 記住0->1瞬間的位置(out_pos),停止旋轉,計算in_pos與out_pos的平均值,再讓馬達慢慢轉到位置 if(hall_input != hall_preinput ) { calibrate_count += 1 ; if(hall_input == 0) hall_in_pos = controller.theta_mech ; else{ hall_out_pos = controller.theta_mech ; hall_mid_pos = (hall_in_pos + hall_out_pos)/2.0f ; } } if(calibrate_count <= 1) cal_pcmd = cal_pcmd + CALIBRATE_DIR*(1.0f/(40000.0f)*CALIBRATE_SPEED ) ; else{ if(CALIBRATE_DIR == 1 ){ if(CALIBRATE_OFFSET == 0){ //keep turning if(controller.theta_mech >= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ; else{//stop cal_pcmd = 0.0f; calibrate_state = 2; //success spi.ZeroPosition(); //count = 0; //歸零 calibrate_count = 0 ; state = MOTOR_MODE; } } else{ if(controller.theta_mech <= hall_mid_pos + CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ; else{ //stop cal_pcmd = 0.0f; calibrate_state = 2; //success spi.ZeroPosition(); //歸零 calibrate_count = 0 ; state = MOTOR_MODE; } } } else if(CALIBRATE_DIR == -1){ if(CALIBRATE_OFFSET == 0){ //keep turning if(controller.theta_mech <= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ; else{//stop cal_pcmd = 0.0f; calibrate_state = 2; //success spi.ZeroPosition(); //歸零 calibrate_count = 0 ; state = MOTOR_MODE; } } else{ //calibrate_offset != 0 if(controller.theta_mech >= hall_mid_pos - CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ; else{ //stop cal_pcmd = 0.0f; calibrate_state = 2; //success spi.ZeroPosition(); //歸零 calibrate_count = 0 ; state = MOTOR_MODE; } } } } cal_pcmd = (cal_pcmd>2*PI) ? cal_pcmd-=2*PI : cal_pcmd ; cal_pcmd = (cal_pcmd < 0) ? cal_pcmd+=2*PI : cal_pcmd ; controller.p_des = cal_pcmd ; } hall_preinput = hall_input ; } /// Check state machine state, and run the appropriate function /// switch(state){ case REST_MODE: // Do nothing if(state_change){ if(calibrate_state != 2) //success enter_menu_state(); else{ drv.disable_gd(); gpio.enable->write(0); state_change = 0; gpio.led->write(0); } } break; case CALIBRATION_MODE: // Run encoder calibration procedure if(state_change){ calibrate(); } break; case MOTOR_MODE: // Run torque control if(state_change){ enter_torque_mode(); count = 0; } else{ /* if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen gpio. ->write(0); controller.ovp_flag = 1; state = REST_MODE; state_change = 1; printf("OVP Triggered!\n\r"); } */ if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; controller.kp = 0; controller.kd = 0; controller.t_ff = 0; } torque_control(&controller); commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout++; count++; } break; case HALL_CALIBRATE: if(state_change){ enter_torque_mode(); count = 0; } else{ if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ controller.i_d_ref = 0; controller.i_q_ref = 0; controller.kp = 0; controller.kd = 0; controller.t_ff = 0; } torque_control(&controller); commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop controller.timeout++; count++; } break; case SETUP_MODE: if(state_change){ enter_setup_state(); } break; case ENCODER_MODE: print_encoder(); break; } } TIM1->SR = 0x0; // reset the status register //GPIO_PID_DEBUG->write(0); } char cmd_val[8] = {0}; char cmd_id = 0; char char_count = 0; /// Manage state machine with commands from serial terminal or configurator gui /// /// Called when data received over serial /// void serial_interrupt(void){ while(pc.readable()){ char c = pc.getc(); if(c == 27){ state = REST_MODE; state_change = 1; char_count = 0; cmd_id = 0; gpio.led->write(0);; for(int i = 0; i<8; i++){cmd_val[i] = 0;} } if(state == REST_MODE){ switch (c){ case 'c': state = CALIBRATION_MODE; state_change = 1; break; case 'm': state = MOTOR_MODE; state_change = 1; break; case 'e': state = ENCODER_MODE; state_change = 1; break; case 's': state = SETUP_MODE; state_change = 1; break; case 'z': spi.SetMechOffset(0); spi.Sample(DT); wait_us(20); M_OFFSET = spi.GetMechPosition(); if (!prefs.ready()) prefs.open(); prefs.flush(); // Write new prefs to flash prefs.close(); prefs.load(); spi.SetMechOffset(M_OFFSET); printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); break; } } else if(state == SETUP_MODE){ if(c == 13){ switch (cmd_id){ case 'b': I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); break; case 'o': CAN_ID = atoi(cmd_val); break; case 'm': CAN_MASTER = atoi(cmd_val); break; case 'l': I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f); break; case 'f': I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f); break; case 't': CAN_TIMEOUT = atoi(cmd_val); break; case 'a': CALIBRATE_DIR = atoi(cmd_val); break; case 'e': CALIBRATE_OFFSET = fmaxf(fminf(atof(cmd_val), 143.0f), 0.0f); break; case 'v': CALIBRATE_SPEED = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f); break; case 'p': MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);; break; case 'i': MOTOR_KI = fmaxf(fminf(atof(cmd_val), KI_MAX), KI_MIN);; break; case 'd': MOTOR_KD = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN); break; default: printf("ASCII: %d\n\r", c); printf("\n\rASCII: %d\n\r", cmd_id); printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); break; } if (!prefs.ready()) prefs.open(); prefs.flush(); // Write new prefs to flash prefs.close(); prefs.load(); /*----- change new pid controller parameter -----*/ controller.kp = MOTOR_KP; controller.ki = MOTOR_KI; controller.kd = MOTOR_KD; state_change = 1; char_count = 0; cmd_id = 0; for(int i = 0; i<8; i++){cmd_val[i] = 0;} } else{ if(char_count == 0){cmd_id = c;} else{ cmd_val[char_count-1] = c; } pc.putc(c); char_count++; } } else if (state == ENCODER_MODE){ switch (c){ case 27: state = REST_MODE; state_change = 1; break; } } else if (state == MOTOR_MODE){ switch (c){ case 'd': controller.i_q_ref = 0; controller.i_d_ref = 0; } } } } void GPIO_SETUP(){ //GPIOB->MODER |= 0x40000000; //GPIOC->MODER |= 0x0000100f; GPIOC->MODER |= 0x00000000; } int main() { controller.v_bus = V_BUS; controller.mode = 0; Init_All_HW(&gpio); // Setup PWM, ADC, GPIO //DEBUG_GPIO(); GPIO_SETUP(); wait(.1); gpio.enable->write(1); wait_us(100); drv.calibrate(); wait_us(100); drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); wait_us(100); 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(); gpio.enable->write(0); wait(.1); /* gpio.enable->write(1); TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles TIM1->CCR2 = 0x708*(1.0f); 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; //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt wait(.1); NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication NVIC_SetPriority(CAN1_RX0_IRQn, 3); // attach 'CAN receive-complete' interrupt handler // If preferences haven't been user configured yet, set defaults prefs.load(); // Read flash can.filter(CAN_ID , 0xFFF, CANStandard, 0); txMsg.id = CAN_MASTER; txMsg.len = 8; rxMsg.len = 8; can.attach(&onMsgReceived); 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_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;} if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;} if(isnan(MOTOR_KP) || MOTOR_KP==-1){MOTOR_KP = 5;} if(isnan(MOTOR_KI) || MOTOR_KI==-1){MOTOR_KI = 0;} if(isnan(MOTOR_KD) || MOTOR_KD==-1){MOTOR_KD = 1.25;} if(CALIBRATE_DIR != -1 && CALIBRATE_DIR != 1){CALIBRATE_DIR = 1;} if(isnan(CALIBRATE_OFFSET) || CALIBRATE_OFFSET==-1){CALIBRATE_OFFSET = 0.0;} if(isnan(CALIBRATE_SPEED) || CALIBRATE_SPEED==-1){CALIBRATE_SPEED = 0.25;} spi.SetElecOffset(E_OFFSET); // Set position sensor offset spi.SetMechOffset(M_OFFSET); int lut[128] = {0}; memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table init_controller_params(&controller); pc.baud(115200); // set serial baud rate wait(.01); pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); wait(.01); printf("\n\r Debug Info:\n\r"); printf(" Firmware Version: %d\n\r", VERSION_NUM); printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); printf(" Output Zero Position: %.4f\n\r", M_OFFSET); printf(" CAN ID: %d\n\r", CAN_ID); /*----- set controller parameters -----*/ controller.kp = MOTOR_KP; controller.ki = MOTOR_KI; controller.kd = MOTOR_KD; printf(" PID controller parameters \n\r"); printf(" KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd); printf(" Calibrate Direction: %d\n\r", CALIBRATE_DIR); printf(" Calibrate Offset: %f\n\r", CALIBRATE_OFFSET); printf(" Calibrate Speed: %f\n\r", CALIBRATE_SPEED); //printf(" %d\n\r", drv.read_register(DCR)); //wait_us(100); //printf(" %d\n\r", drv.read_register(CSACR)); //wait_us(100); //printf(" %d\n\r", drv.read_register(OCPCR)); //drv.disable_gd(); pc.attach(&serial_interrupt); // attach serial interrupt state_change = 1; // calibrate calibrate_state = 0; int counter = 0; while(1) { // if(complete_changepara){ // printf(" %-4s %-31s %d\n\r", "o", "CAN_ID", CAN_ID); // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP); // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI); // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD); // complete_changepara = false; // state = REST_MODE; // state_change = 1; // } // printf("Hall_input(PC6): %d\n\r", hall_input); // printf("Hall_preinput(PC6): %d\n\r", hall_preinput); // printf("calibrate count: %d\n\r",calibrate_count); // printf("cal_pcmd: %.4f\n\r",cal_pcmd); // printf("pos_in: %f\n\r",hall_in_pos); // printf("pos_out: %f\n\r",hall_out_pos); // printf("theta_mech: %f\n\r",controller.theta_mech); // printf("controller.pdes: %f\n\r", controller.p_des); // printf("state: %d\n\r",state); // wait(1); /* CAN_DEBUG->write(1); pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM); can_state = can.write(txMsg); CAN_DEBUG->write(can_state); printf("helloworld\n\r"); wait(1); */ /* CAN_DEBUG->write(1); wait(0.1); CAN_DEBUG->write(0); wait(0.1); */ //drv.print_faults(); //wait(.1); //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); } */ } }