1
Dependencies: mbed-dev-f303 FastPWM3
main.cpp@56:d34e4540ec12, 2020-08-14 (annotated)
- Committer:
- Jasper_gu
- Date:
- Fri Aug 14 07:22:11 2020 +0000
- Revision:
- 56:d34e4540ec12
- Parent:
- 52:91a42bd0fe2e
uuuu
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 22:60276ba87ac6 | 1 | /// high-bandwidth 3-phase motor control, for robots |
benkatz | 45:26801179208e | 2 | /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others |
benkatz | 22:60276ba87ac6 | 3 | /// Hardware documentation can be found at build-its.blogspot.com |
benkatz | 22:60276ba87ac6 | 4 | /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling |
benkatz | 47:e1196a851f76 | 5 | /// Version for the TI DRV8323 Everything Chip |
benkatz | 22:60276ba87ac6 | 6 | |
benkatz | 23:2adf23ee0305 | 7 | #define REST_MODE 0 |
benkatz | 23:2adf23ee0305 | 8 | #define CALIBRATION_MODE 1 |
benkatz | 26:2b865c00d7e9 | 9 | #define MOTOR_MODE 2 |
benkatz | 23:2adf23ee0305 | 10 | #define SETUP_MODE 4 |
benkatz | 23:2adf23ee0305 | 11 | #define ENCODER_MODE 5 |
benkatz | 22:60276ba87ac6 | 12 | |
Rushu | 51:b0a3ef66ea3d | 13 | #define VERSION_NUM "0.1" |
benkatz | 26:2b865c00d7e9 | 14 | |
benkatz | 18:f1d56f4acb39 | 15 | |
benkatz | 26:2b865c00d7e9 | 16 | float __float_reg[64]; // Floats stored in flash |
benkatz | 26:2b865c00d7e9 | 17 | int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table |
benkatz | 17:3c5df2982199 | 18 | |
benkatz | 0:4e1c4df6aabd | 19 | #include "mbed.h" |
benkatz | 0:4e1c4df6aabd | 20 | #include "PositionSensor.h" |
benkatz | 20:bf9ea5125d52 | 21 | #include "structs.h" |
benkatz | 20:bf9ea5125d52 | 22 | #include "foc.h" |
benkatz | 22:60276ba87ac6 | 23 | #include "calibration.h" |
benkatz | 20:bf9ea5125d52 | 24 | #include "hw_setup.h" |
benkatz | 23:2adf23ee0305 | 25 | #include "math_ops.h" |
benkatz | 20:bf9ea5125d52 | 26 | #include "current_controller_config.h" |
benkatz | 20:bf9ea5125d52 | 27 | #include "hw_config.h" |
benkatz | 20:bf9ea5125d52 | 28 | #include "motor_config.h" |
benkatz | 23:2adf23ee0305 | 29 | #include "stm32f4xx_flash.h" |
benkatz | 23:2adf23ee0305 | 30 | #include "FlashWriter.h" |
benkatz | 23:2adf23ee0305 | 31 | #include "user_config.h" |
benkatz | 23:2adf23ee0305 | 32 | #include "PreferenceWriter.h" |
benkatz | 42:738fa01b0346 | 33 | #include "CAN_com.h" |
benkatz | 44:8040fa2fcb0d | 34 | #include "DRV.h" |
benkatz | 26:2b865c00d7e9 | 35 | |
benkatz | 23:2adf23ee0305 | 36 | PreferenceWriter prefs(6); |
benkatz | 9:d7eb815cb057 | 37 | |
benkatz | 20:bf9ea5125d52 | 38 | GPIOStruct gpio; |
benkatz | 20:bf9ea5125d52 | 39 | ControllerStruct controller; |
benkatz | 48:74a40481740c | 40 | ObserverStruct observer; |
benkatz | 20:bf9ea5125d52 | 41 | COMStruct com; |
benkatz | 43:dfb72608639c | 42 | Serial pc(PA_2, PA_3); |
benkatz | 9:d7eb815cb057 | 43 | |
benkatz | 17:3c5df2982199 | 44 | |
benkatz | 45:26801179208e | 45 | CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name |
benkatz | 26:2b865c00d7e9 | 46 | CANMessage rxMsg; |
benkatz | 26:2b865c00d7e9 | 47 | CANMessage txMsg; |
benkatz | 23:2adf23ee0305 | 48 | |
Jasper_gu | 56:d34e4540ec12 | 49 | PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg |
benkatz | 20:bf9ea5125d52 | 50 | |
Jasper_gu | 56:d34e4540ec12 | 51 | //drv8323 SPI 设置 改为PC12\11\10 SPI3,片选改为PA15 |
Jasper_gu | 56:d34e4540ec12 | 52 | SPI drv_spi(PC_12, PC_11, PC_10); |
benkatz | 8:10ae7bc88d6e | 53 | |
Jasper_gu | 56:d34e4540ec12 | 54 | // GPIOC->AFR[1]&=0xfff000ff; |
Jasper_gu | 56:d34e4540ec12 | 55 | // GPIOC->AFR[1]|=0x00055500; |
Jasper_gu | 56:d34e4540ec12 | 56 | DigitalOut drv_cs(PA_15); |
Jasper_gu | 56:d34e4540ec12 | 57 | |
Jasper_gu | 56:d34e4540ec12 | 58 | DRV832x drv(&drv_spi, &drv_cs); |
Jasper_gu | 56:d34e4540ec12 | 59 | |
Jasper_gu | 56:d34e4540ec12 | 60 | |
benkatz | 20:bf9ea5125d52 | 61 | |
benkatz | 23:2adf23ee0305 | 62 | volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 63 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 64 | volatile int state_change; |
benkatz | 20:bf9ea5125d52 | 65 | |
Rushu | 51:b0a3ef66ea3d | 66 | //================HJB=============added========== |
Rushu | 51:b0a3ef66ea3d | 67 | using namespace FastMath; |
Rushu | 51:b0a3ef66ea3d | 68 | volatile float Init_pos = 0; |
Rushu | 51:b0a3ef66ea3d | 69 | volatile float Pmag = 1; |
Rushu | 51:b0a3ef66ea3d | 70 | volatile float Tperiod = 25; |
Rushu | 51:b0a3ef66ea3d | 71 | volatile float p_des_HJB=0; |
Rushu | 51:b0a3ef66ea3d | 72 | volatile float v_des_HJB=0; |
Rushu | 51:b0a3ef66ea3d | 73 | |
Rushu | 51:b0a3ef66ea3d | 74 | //===================HJB end=================== |
Rushu | 51:b0a3ef66ea3d | 75 | |
benkatz | 26:2b865c00d7e9 | 76 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 77 | //msgAvailable = true; |
benkatz | 47:e1196a851f76 | 78 | printf("%df\n\r", rxMsg.id); |
benkatz | 26:2b865c00d7e9 | 79 | can.read(rxMsg); |
benkatz | 28:8c7e29f719c5 | 80 | if((rxMsg.id == CAN_ID)){ |
benkatz | 28:8c7e29f719c5 | 81 | controller.timeout = 0; |
benkatz | 28:8c7e29f719c5 | 82 | 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))){ |
benkatz | 28:8c7e29f719c5 | 83 | state = MOTOR_MODE; |
benkatz | 28:8c7e29f719c5 | 84 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 85 | } |
benkatz | 28:8c7e29f719c5 | 86 | 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))){ |
benkatz | 28:8c7e29f719c5 | 87 | state = REST_MODE; |
benkatz | 28:8c7e29f719c5 | 88 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 89 | gpio.led->write(0);; |
benkatz | 28:8c7e29f719c5 | 90 | } |
benkatz | 28:8c7e29f719c5 | 91 | 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))){ |
benkatz | 28:8c7e29f719c5 | 92 | spi.ZeroPosition(); |
benkatz | 28:8c7e29f719c5 | 93 | } |
benkatz | 28:8c7e29f719c5 | 94 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 95 | unpack_cmd(rxMsg, &controller); |
benkatz | 28:8c7e29f719c5 | 96 | } |
benkatz | 37:c0f352d6e8e3 | 97 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); |
benkatz | 37:c0f352d6e8e3 | 98 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 99 | } |
benkatz | 26:2b865c00d7e9 | 100 | |
benkatz | 26:2b865c00d7e9 | 101 | } |
benkatz | 26:2b865c00d7e9 | 102 | |
benkatz | 23:2adf23ee0305 | 103 | void enter_menu_state(void){ |
benkatz | 44:8040fa2fcb0d | 104 | drv.disable_gd(); |
benkatz | 47:e1196a851f76 | 105 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 106 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 107 | printf(" Commands:\n\r"); |
benkatz | 44:8040fa2fcb0d | 108 | wait_us(10); |
benkatz | 26:2b865c00d7e9 | 109 | printf(" m - Motor Mode\n\r"); |
benkatz | 44:8040fa2fcb0d | 110 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 111 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 112 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 113 | printf(" s - Setup\n\r"); |
benkatz | 44:8040fa2fcb0d | 114 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 115 | printf(" e - Display Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 116 | wait_us(10); |
benkatz | 37:c0f352d6e8e3 | 117 | printf(" z - Set Zero Position\n\r"); |
benkatz | 44:8040fa2fcb0d | 118 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 119 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 44:8040fa2fcb0d | 120 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 121 | state_change = 0; |
benkatz | 37:c0f352d6e8e3 | 122 | gpio.led->write(0); |
benkatz | 23:2adf23ee0305 | 123 | } |
benkatz | 24:58c2d7571207 | 124 | |
benkatz | 24:58c2d7571207 | 125 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 126 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 44:8040fa2fcb0d | 127 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 128 | printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:8040fa2fcb0d | 129 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 130 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 44:8040fa2fcb0d | 131 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 132 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 44:8040fa2fcb0d | 133 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 134 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
benkatz | 44:8040fa2fcb0d | 135 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 136 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); |
benkatz | 44:8040fa2fcb0d | 137 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 138 | printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
benkatz | 44:8040fa2fcb0d | 139 | wait_us(10); |
benkatz | 24:58c2d7571207 | 140 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 44:8040fa2fcb0d | 141 | wait_us(10); |
benkatz | 24:58c2d7571207 | 142 | state_change = 0; |
benkatz | 24:58c2d7571207 | 143 | } |
benkatz | 22:60276ba87ac6 | 144 | |
benkatz | 23:2adf23ee0305 | 145 | void enter_torque_mode(void){ |
benkatz | 44:8040fa2fcb0d | 146 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 147 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 148 | controller.ovp_flag = 0; |
Rushu | 51:b0a3ef66ea3d | 149 | controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off. |
Jasper_gu | 56:d34e4540ec12 | 150 | reset_foc(&controller); // Resets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 151 | wait(.001); |
benkatz | 23:2adf23ee0305 | 152 | controller.i_d_ref = 0; |
benkatz | 50:ba72df25d10f | 153 | controller.i_q_ref = 0; // Current Setpoints |
benkatz | 37:c0f352d6e8e3 | 154 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 155 | state_change = 0; |
benkatz | 28:8c7e29f719c5 | 156 | printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 157 | } |
benkatz | 22:60276ba87ac6 | 158 | |
benkatz | 23:2adf23ee0305 | 159 | void calibrate(void){ |
benkatz | 44:8040fa2fcb0d | 160 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 161 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 162 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 163 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 164 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
benkatz | 37:c0f352d6e8e3 | 165 | gpio.led->write(0);; // Turn off status LED |
benkatz | 23:2adf23ee0305 | 166 | wait(.2); |
benkatz | 23:2adf23ee0305 | 167 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 44:8040fa2fcb0d | 168 | drv.disable_gd(); |
benkatz | 47:e1196a851f76 | 169 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 170 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 171 | } |
benkatz | 23:2adf23ee0305 | 172 | |
benkatz | 23:2adf23ee0305 | 173 | void print_encoder(void){ |
Rushu | 51:b0a3ef66ea3d | 174 | printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //spi.GetMechPosition |
Jasper_gu | 56:d34e4540ec12 | 175 | // printf("%d %d %d \n\r",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw); |
benkatz | 48:74a40481740c | 176 | //printf("%d\n\r", spi.GetRawPosition()); |
Jasper_gu | 56:d34e4540ec12 | 177 | // can.write(txMsg); |
benkatz | 47:e1196a851f76 | 178 | wait(.001); |
benkatz | 22:60276ba87ac6 | 179 | } |
benkatz | 20:bf9ea5125d52 | 180 | |
benkatz | 23:2adf23ee0305 | 181 | /// Current Sampling Interrupt /// |
Rushu | 51:b0a3ef66ea3d | 182 | /// This runs at 40 kHz, 25us, regardless of of the mode the controller is in /// |
benkatz | 2:8724412ad628 | 183 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 184 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 23:2adf23ee0305 | 185 | |
benkatz | 23:2adf23ee0305 | 186 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 187 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 188 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 189 | //for (delay = 0; delay < 55; delay++); |
benkatz | 45:26801179208e | 190 | |
benkatz | 47:e1196a851f76 | 191 | spi.Sample(DT); // sample position sensor |
benkatz | 37:c0f352d6e8e3 | 192 | controller.adc2_raw = ADC2->DR; // Read ADC Data Registers |
benkatz | 23:2adf23ee0305 | 193 | controller.adc1_raw = ADC1->DR; |
benkatz | 37:c0f352d6e8e3 | 194 | controller.adc3_raw = ADC3->DR; |
benkatz | 37:c0f352d6e8e3 | 195 | controller.theta_elec = spi.GetElecPosition(); |
benkatz | 37:c0f352d6e8e3 | 196 | controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 197 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); |
benkatz | 37:c0f352d6e8e3 | 198 | controller.dtheta_elec = spi.GetElecVelocity(); |
Rushu | 51:b0a3ef66ea3d | 199 | controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //HJB find, new ADC? V_SCALE 0.012890625f |
benkatz | 23:2adf23ee0305 | 200 | /// |
benkatz | 20:bf9ea5125d52 | 201 | |
benkatz | 23:2adf23ee0305 | 202 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 203 | switch(state){ |
benkatz | 37:c0f352d6e8e3 | 204 | case REST_MODE: // Do nothing |
benkatz | 23:2adf23ee0305 | 205 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 206 | enter_menu_state(); |
benkatz | 23:2adf23ee0305 | 207 | } |
benkatz | 23:2adf23ee0305 | 208 | break; |
benkatz | 22:60276ba87ac6 | 209 | |
benkatz | 23:2adf23ee0305 | 210 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 211 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 212 | calibrate(); |
benkatz | 23:2adf23ee0305 | 213 | } |
benkatz | 23:2adf23ee0305 | 214 | break; |
benkatz | 23:2adf23ee0305 | 215 | |
benkatz | 26:2b865c00d7e9 | 216 | case MOTOR_MODE: // Run torque control |
Jasper_gu | 56:d34e4540ec12 | 217 | if(state_change){//Initiate Torque control parameters |
benkatz | 25:f5741040c4bb | 218 | enter_torque_mode(); |
benkatz | 28:8c7e29f719c5 | 219 | count = 0; |
Rushu | 51:b0a3ef66ea3d | 220 | //===============================================HJB added====================================================// |
Rushu | 51:b0a3ef66ea3d | 221 | Init_pos = controller.theta_mech; |
Rushu | 51:b0a3ef66ea3d | 222 | // printf(" Mechanical Angle: %f \n\r", Init_pos); //spi.GetMechPosition |
benkatz | 25:f5741040c4bb | 223 | } |
benkatz | 28:8c7e29f719c5 | 224 | else{ |
benkatz | 37:c0f352d6e8e3 | 225 | /* |
benkatz | 37:c0f352d6e8e3 | 226 | 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 |
benkatz | 47:e1196a851f76 | 227 | gpio. |
benkatz | 47:e1196a851f76 | 228 | ->write(0); |
benkatz | 37:c0f352d6e8e3 | 229 | controller.ovp_flag = 1; |
benkatz | 37:c0f352d6e8e3 | 230 | state = REST_MODE; |
benkatz | 37:c0f352d6e8e3 | 231 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 232 | printf("OVP Triggered!\n\r"); |
benkatz | 37:c0f352d6e8e3 | 233 | } |
benkatz | 37:c0f352d6e8e3 | 234 | */ |
Rushu | 51:b0a3ef66ea3d | 235 | //========================================HJB added for trajectory input=========================================// |
Rushu | 51:b0a3ef66ea3d | 236 | Pmag = controller.PmagIn; |
Rushu | 51:b0a3ef66ea3d | 237 | Tperiod = controller.TperiodIn; |
Rushu | 51:b0a3ef66ea3d | 238 | p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count/(Tperiod*40000));//Pmag*FastSin(2*PI*count/(Tperiod*40000)); |
Rushu | 51:b0a3ef66ea3d | 239 | v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count/(Tperiod*40000))/Tperiod; |
Rushu | 52:91a42bd0fe2e | 240 | // controller.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16); |
Rushu | 52:91a42bd0fe2e | 241 | // controller.v_des = v_des_HJB; |
Rushu | 51:b0a3ef66ea3d | 242 | if(count>=(Tperiod*40000)) |
Rushu | 51:b0a3ef66ea3d | 243 | {count = 0;} |
Rushu | 51:b0a3ef66ea3d | 244 | |
Rushu | 51:b0a3ef66ea3d | 245 | //========================================HJB end=========================================// |
benkatz | 28:8c7e29f719c5 | 246 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 247 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 248 | controller.i_q_ref = 0; |
benkatz | 37:c0f352d6e8e3 | 249 | controller.kp = 0; |
benkatz | 37:c0f352d6e8e3 | 250 | controller.kd = 0; |
benkatz | 37:c0f352d6e8e3 | 251 | controller.t_ff = 0; |
Rushu | 51:b0a3ef66ea3d | 252 | //controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off. |
Rushu | 51:b0a3ef66ea3d | 253 | controller.v_des = 0; //HJB added |
benkatz | 28:8c7e29f719c5 | 254 | } |
Rushu | 51:b0a3ef66ea3d | 255 | |
Jasper_gu | 56:d34e4540ec12 | 256 | torque_control(&controller);//Calculate the desire torque for current loop |
benkatz | 49:83d83040ea51 | 257 | commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop |
benkatz | 49:83d83040ea51 | 258 | |
benkatz | 49:83d83040ea51 | 259 | controller.timeout++; |
benkatz | 49:83d83040ea51 | 260 | count++; |
benkatz | 37:c0f352d6e8e3 | 261 | |
benkatz | 37:c0f352d6e8e3 | 262 | } |
benkatz | 23:2adf23ee0305 | 263 | break; |
benkatz | 23:2adf23ee0305 | 264 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 265 | if(state_change){ |
benkatz | 24:58c2d7571207 | 266 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 267 | } |
benkatz | 23:2adf23ee0305 | 268 | break; |
benkatz | 23:2adf23ee0305 | 269 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 270 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 271 | break; |
benkatz | 37:c0f352d6e8e3 | 272 | } |
benkatz | 2:8724412ad628 | 273 | } |
benkatz | 23:2adf23ee0305 | 274 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 275 | } |
benkatz | 0:4e1c4df6aabd | 276 | |
benkatz | 25:f5741040c4bb | 277 | |
benkatz | 24:58c2d7571207 | 278 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 279 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 280 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 281 | |
benkatz | 25:f5741040c4bb | 282 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 283 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 284 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 285 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 286 | char c = pc.getc(); |
Jasper_gu | 56:d34e4540ec12 | 287 | if(c == 27){ //return to default mode |
Rushu | 51:b0a3ef66ea3d | 288 | //===============================================HJB added====================================================// |
Rushu | 51:b0a3ef66ea3d | 289 | wait_us(100); //HJB add, to clear fault |
Rushu | 51:b0a3ef66ea3d | 290 | drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault |
Rushu | 51:b0a3ef66ea3d | 291 | Init_pos = controller.theta_mech; //Input the local mechanical theta |
Rushu | 51:b0a3ef66ea3d | 292 | //===============================================HJB ended====================================================// |
benkatz | 25:f5741040c4bb | 293 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 294 | state_change = 1; |
benkatz | 25:f5741040c4bb | 295 | char_count = 0; |
benkatz | 25:f5741040c4bb | 296 | cmd_id = 0; |
benkatz | 37:c0f352d6e8e3 | 297 | gpio.led->write(0);; |
benkatz | 25:f5741040c4bb | 298 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 299 | } |
benkatz | 24:58c2d7571207 | 300 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 301 | switch (c){ |
benkatz | 23:2adf23ee0305 | 302 | case 'c': |
benkatz | 23:2adf23ee0305 | 303 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 304 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 305 | break; |
benkatz | 26:2b865c00d7e9 | 306 | case 'm': |
benkatz | 26:2b865c00d7e9 | 307 | state = MOTOR_MODE; |
benkatz | 23:2adf23ee0305 | 308 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 309 | break; |
benkatz | 23:2adf23ee0305 | 310 | case 'e': |
benkatz | 23:2adf23ee0305 | 311 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 312 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 313 | break; |
Jasper_gu | 56:d34e4540ec12 | 314 | case 's':// |
benkatz | 23:2adf23ee0305 | 315 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 316 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 317 | break; |
Jasper_gu | 56:d34e4540ec12 | 318 | case 'z'://set zero position |
benkatz | 37:c0f352d6e8e3 | 319 | spi.SetMechOffset(0); |
benkatz | 47:e1196a851f76 | 320 | spi.Sample(DT); |
benkatz | 37:c0f352d6e8e3 | 321 | wait_us(20); |
benkatz | 37:c0f352d6e8e3 | 322 | M_OFFSET = spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 323 | if (!prefs.ready()) prefs.open(); |
benkatz | 37:c0f352d6e8e3 | 324 | prefs.flush(); // Write new prefs to flash |
benkatz | 37:c0f352d6e8e3 | 325 | prefs.close(); |
benkatz | 37:c0f352d6e8e3 | 326 | prefs.load(); |
benkatz | 37:c0f352d6e8e3 | 327 | spi.SetMechOffset(M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 328 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 329 | |
benkatz | 37:c0f352d6e8e3 | 330 | break; |
benkatz | 37:c0f352d6e8e3 | 331 | } |
benkatz | 37:c0f352d6e8e3 | 332 | |
benkatz | 24:58c2d7571207 | 333 | } |
benkatz | 24:58c2d7571207 | 334 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 335 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 336 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 337 | case 'b': |
benkatz | 24:58c2d7571207 | 338 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 339 | break; |
benkatz | 24:58c2d7571207 | 340 | case 'i': |
benkatz | 24:58c2d7571207 | 341 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 342 | break; |
benkatz | 26:2b865c00d7e9 | 343 | case 'm': |
benkatz | 26:2b865c00d7e9 | 344 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 345 | break; |
benkatz | 24:58c2d7571207 | 346 | case 'l': |
benkatz | 24:58c2d7571207 | 347 | TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 348 | break; |
benkatz | 28:8c7e29f719c5 | 349 | case 't': |
Rushu | 51:b0a3ef66ea3d | 350 | CAN_TIMEOUT = atoi(cmd_val); //100000*25us= 2.5s |
benkatz | 28:8c7e29f719c5 | 351 | break; |
benkatz | 24:58c2d7571207 | 352 | default: |
benkatz | 24:58c2d7571207 | 353 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 354 | break; |
benkatz | 24:58c2d7571207 | 355 | } |
benkatz | 24:58c2d7571207 | 356 | |
benkatz | 24:58c2d7571207 | 357 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 358 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 359 | prefs.close(); |
benkatz | 24:58c2d7571207 | 360 | prefs.load(); |
benkatz | 24:58c2d7571207 | 361 | state_change = 1; |
benkatz | 24:58c2d7571207 | 362 | char_count = 0; |
benkatz | 24:58c2d7571207 | 363 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 364 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 365 | } |
benkatz | 24:58c2d7571207 | 366 | else{ |
benkatz | 24:58c2d7571207 | 367 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 368 | else{ |
benkatz | 24:58c2d7571207 | 369 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 370 | |
benkatz | 24:58c2d7571207 | 371 | } |
benkatz | 24:58c2d7571207 | 372 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 373 | char_count++; |
benkatz | 23:2adf23ee0305 | 374 | } |
benkatz | 23:2adf23ee0305 | 375 | } |
benkatz | 24:58c2d7571207 | 376 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 377 | switch (c){ |
benkatz | 24:58c2d7571207 | 378 | case 27: |
benkatz | 24:58c2d7571207 | 379 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 380 | state_change = 1; |
benkatz | 24:58c2d7571207 | 381 | break; |
benkatz | 24:58c2d7571207 | 382 | } |
benkatz | 24:58c2d7571207 | 383 | } |
benkatz | 49:83d83040ea51 | 384 | else if (state == MOTOR_MODE){ |
benkatz | 49:83d83040ea51 | 385 | switch (c){ |
benkatz | 49:83d83040ea51 | 386 | case 'd': |
benkatz | 49:83d83040ea51 | 387 | controller.i_q_ref = 0; |
benkatz | 49:83d83040ea51 | 388 | controller.i_d_ref = 0; |
benkatz | 49:83d83040ea51 | 389 | } |
benkatz | 49:83d83040ea51 | 390 | } |
benkatz | 24:58c2d7571207 | 391 | |
benkatz | 24:58c2d7571207 | 392 | } |
benkatz | 22:60276ba87ac6 | 393 | } |
benkatz | 0:4e1c4df6aabd | 394 | |
benkatz | 0:4e1c4df6aabd | 395 | int main() { |
Rushu | 51:b0a3ef66ea3d | 396 | wait(1); |
benkatz | 20:bf9ea5125d52 | 397 | controller.v_bus = V_BUS; |
Rushu | 51:b0a3ef66ea3d | 398 | controller.PmagIn = 1; |
Rushu | 51:b0a3ef66ea3d | 399 | controller.TperiodIn = 10; |
benkatz | 22:60276ba87ac6 | 400 | controller.mode = 0; |
benkatz | 23:2adf23ee0305 | 401 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 44:8040fa2fcb0d | 402 | wait(.1); |
benkatz | 44:8040fa2fcb0d | 403 | |
Rushu | 51:b0a3ef66ea3d | 404 | gpio.enable->write(1); //enable! |
benkatz | 44:8040fa2fcb0d | 405 | wait_us(100); |
Jasper_gu | 56:d34e4540ec12 | 406 | drv.calibrate(); //calibrete and initialize the Drv8323 Chip |
benkatz | 45:26801179208e | 407 | wait_us(100); |
benkatz | 44:8040fa2fcb0d | 408 | drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); |
benkatz | 44:8040fa2fcb0d | 409 | wait_us(100); |
Rushu | 51:b0a3ef66ea3d | 410 | drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); //SEN_LVL_1_0 0x3 |
benkatz | 44:8040fa2fcb0d | 411 | wait_us(100); |
benkatz | 49:83d83040ea51 | 412 | drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88); |
Rushu | 51:b0a3ef66ea3d | 413 | |
benkatz | 45:26801179208e | 414 | //drv.enable_gd(); |
benkatz | 44:8040fa2fcb0d | 415 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
Rushu | 51:b0a3ef66ea3d | 416 | drv.disable_gd(); //all mosfets in the Hi-z state |
benkatz | 20:bf9ea5125d52 | 417 | |
benkatz | 44:8040fa2fcb0d | 418 | |
benkatz | 44:8040fa2fcb0d | 419 | |
benkatz | 44:8040fa2fcb0d | 420 | |
benkatz | 9:d7eb815cb057 | 421 | wait(.1); |
benkatz | 44:8040fa2fcb0d | 422 | /* |
benkatz | 26:2b865c00d7e9 | 423 | gpio.enable->write(1); |
benkatz | 26:2b865c00d7e9 | 424 | TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles |
benkatz | 26:2b865c00d7e9 | 425 | TIM1->CCR2 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 426 | TIM1->CCR1 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 427 | gpio.enable->write(0); |
benkatz | 44:8040fa2fcb0d | 428 | */ |
Jasper_gu | 56:d34e4540ec12 | 429 | reset_foc(&controller); // Reset current controller |
benkatz | 48:74a40481740c | 430 | reset_observer(&observer); // Reset observer |
benkatz | 26:2b865c00d7e9 | 431 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 26:2b865c00d7e9 | 432 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 433 | |
benkatz | 20:bf9ea5125d52 | 434 | wait(.1); |
benkatz | 37:c0f352d6e8e3 | 435 | NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication |
benkatz | 43:dfb72608639c | 436 | |
benkatz | 37:c0f352d6e8e3 | 437 | NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
benkatz | 26:2b865c00d7e9 | 438 | can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
benkatz | 43:dfb72608639c | 439 | |
benkatz | 28:8c7e29f719c5 | 440 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 441 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 442 | rxMsg.len = 8; |
benkatz | 43:dfb72608639c | 443 | can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler |
benkatz | 23:2adf23ee0305 | 444 | |
benkatz | 48:74a40481740c | 445 | // If preferences haven't been user configured yet, set defaults |
benkatz | 25:f5741040c4bb | 446 | prefs.load(); // Read flash |
benkatz | 37:c0f352d6e8e3 | 447 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
benkatz | 37:c0f352d6e8e3 | 448 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
benkatz | 48:74a40481740c | 449 | if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} |
benkatz | 48:74a40481740c | 450 | if(isnan(TORQUE_LIMIT) || TORQUE_LIMIT ==-1){TORQUE_LIMIT=18;} |
benkatz | 48:74a40481740c | 451 | if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} |
benkatz | 48:74a40481740c | 452 | if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} |
benkatz | 48:74a40481740c | 453 | if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;} |
benkatz | 25:f5741040c4bb | 454 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 37:c0f352d6e8e3 | 455 | spi.SetMechOffset(M_OFFSET); |
benkatz | 23:2adf23ee0305 | 456 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 457 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 25:f5741040c4bb | 458 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
benkatz | 45:26801179208e | 459 | init_controller_params(&controller); |
benkatz | 45:26801179208e | 460 | |
Rushu | 51:b0a3ef66ea3d | 461 | pc.baud(115200); // set serial baud rate |
benkatz | 20:bf9ea5125d52 | 462 | wait(.01); |
Rushu | 51:b0a3ef66ea3d | 463 | pc.printf("\n\r\n\r High-integated Joint tobe Better\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 464 | wait(.01); |
benkatz | 23:2adf23ee0305 | 465 | printf("\n\r Debug Info:\n\r"); |
benkatz | 32:ccac5da77844 | 466 | printf(" Firmware Version: %s\n\r", VERSION_NUM); |
benkatz | 23:2adf23ee0305 | 467 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 468 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 469 | printf(" Output Zero Position: %.4f\n\r", M_OFFSET); |
benkatz | 24:58c2d7571207 | 470 | printf(" CAN ID: %d\n\r", CAN_ID); |
benkatz | 44:8040fa2fcb0d | 471 | |
benkatz | 44:8040fa2fcb0d | 472 | |
benkatz | 44:8040fa2fcb0d | 473 | |
benkatz | 44:8040fa2fcb0d | 474 | |
benkatz | 47:e1196a851f76 | 475 | //printf(" %d\n\r", drv.read_register(DCR)); |
benkatz | 47:e1196a851f76 | 476 | //wait_us(100); |
benkatz | 47:e1196a851f76 | 477 | //printf(" %d\n\r", drv.read_register(CSACR)); |
benkatz | 47:e1196a851f76 | 478 | //wait_us(100); |
benkatz | 47:e1196a851f76 | 479 | //printf(" %d\n\r", drv.read_register(OCPCR)); |
benkatz | 47:e1196a851f76 | 480 | //drv.disable_gd(); |
benkatz | 44:8040fa2fcb0d | 481 | |
benkatz | 23:2adf23ee0305 | 482 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 22:60276ba87ac6 | 483 | |
benkatz | 23:2adf23ee0305 | 484 | state_change = 1; |
benkatz | 50:ba72df25d10f | 485 | |
benkatz | 20:bf9ea5125d52 | 486 | |
Jasper_gu | 56:d34e4540ec12 | 487 | // int counter = 0; |
Rushu | 51:b0a3ef66ea3d | 488 | //===============================================HJB added====================================================// |
Rushu | 51:b0a3ef66ea3d | 489 | wait_us(100); //HJB add, to clear fault |
Rushu | 51:b0a3ef66ea3d | 490 | drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault |
Rushu | 51:b0a3ef66ea3d | 491 | //Init_pos = controller.theta_mech; //Input the local mechanical theta |
Rushu | 51:b0a3ef66ea3d | 492 | //===============================================HJB ended====================================================// |
benkatz | 0:4e1c4df6aabd | 493 | while(1) { |
benkatz | 47:e1196a851f76 | 494 | drv.print_faults(); |
Jasper_gu | 56:d34e4540ec12 | 495 | wait(.1); |
benkatz | 48:74a40481740c | 496 | //printf("%.4f\n\r", controller.v_bus); |
Rushu | 51:b0a3ef66ea3d | 497 | |
benkatz | 47:e1196a851f76 | 498 | if(state == MOTOR_MODE) |
benkatz | 47:e1196a851f76 | 499 | { |
Rushu | 52:91a42bd0fe2e | 500 | //printf(" %.3f %.3f %.3f \n\r", controller.p_des,controller.p_des-controller.theta_mech,controller.v_des-controller.dtheta_mech); //spi.GetMechPosition |
Jasper_gu | 56:d34e4540ec12 | 501 | printf(" %.3f %d %d \n\r", controller.v_bus,controller.adc1_raw,controller.adc2_raw); //spi.GetMechPosition |
Rushu | 51:b0a3ef66ea3d | 502 | // printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); |
benkatz | 49:83d83040ea51 | 503 | //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); |
Rushu | 51:b0a3ef66ea3d | 504 | // printf("%.3f\n\r", controller.dtheta_mech); |
Rushu | 51:b0a3ef66ea3d | 505 | wait(.002); |
Rushu | 51:b0a3ef66ea3d | 506 | } |
Jasper_gu | 56:d34e4540ec12 | 507 | // spi.Sample(DT); |
Jasper_gu | 56:d34e4540ec12 | 508 | // print_encoder(); |
Jasper_gu | 56:d34e4540ec12 | 509 | wait(0.01); |
Jasper_gu | 56:d34e4540ec12 | 510 | // printf(" %d\n\r", drv.read_register(DCR)); |
Rushu | 51:b0a3ef66ea3d | 511 | /* |
Rushu | 51:b0a3ef66ea3d | 512 | if(state == MOTOR_MODE) |
Rushu | 51:b0a3ef66ea3d | 513 | { |
Rushu | 51:b0a3ef66ea3d | 514 | //================HJB added======================================================================================= |
Rushu | 51:b0a3ef66ea3d | 515 | printf("\n\Temperature Observer\n\r"); |
Rushu | 51:b0a3ef66ea3d | 516 | printf("%f %f \n\r\n\r", observer->temperature , observer->resistance); |
Rushu | 51:b0a3ef66ea3d | 517 | //====HJB added end================================================================================================= |
Rushu | 51:b0a3ef66ea3d | 518 | //========HJB added================================================================================== |
Rushu | 51:b0a3ef66ea3d | 519 | float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); |
Rushu | 51:b0a3ef66ea3d | 520 | printf("\n\rCurrent\n\r"); |
Rushu | 51:b0a3ef66ea3d | 521 | printf("%f %f %f\n\r\n\r", controller->i_d_filt, controller->i_q_filt, current); |
Rushu | 51:b0a3ef66ea3d | 522 | //====HJB added end====================================================================================== |
Rushu | 51:b0a3ef66ea3d | 523 | //================HJB added======================================================================================= |
Rushu | 51:b0a3ef66ea3d | 524 | printf("\n\Id Iq refrence input Observer\n\r"); |
Rushu | 51:b0a3ef66ea3d | 525 | printf("%f %f %f\n\r\n\r", controller->i_d_ref , controller->i_q_ref, controller->v_bus); |
Rushu | 51:b0a3ef66ea3d | 526 | //====HJB added end================================================================================================= |
Rushu | 51:b0a3ef66ea3d | 527 | wait(.002); |
benkatz | 47:e1196a851f76 | 528 | } |
benkatz | 50:ba72df25d10f | 529 | */ |
benkatz | 0:4e1c4df6aabd | 530 | } |
benkatz | 0:4e1c4df6aabd | 531 | } |