GT_MOTOR_24NM_V03_PT1000
Dependencies: mbed Hobbyking_Cheetah FastPWM3
main.cpp@55:165927e4f815, 2022-05-10 (annotated)
- Committer:
- qiudehua
- Date:
- Tue May 10 02:26:27 2022 +0000
- Revision:
- 55:165927e4f815
- Parent:
- 53:e85efce8c1eb
GT_MOTOR_24NM_V03_PT1000
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 | |
qiudehua | 55:165927e4f815 | 13 | #define VERSION_NUM "0.3" |
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 | |
benkatz | 20:bf9ea5125d52 | 49 | |
benkatz | 44:8040fa2fcb0d | 50 | SPI drv_spi(PA_7, PA_6, PA_5); |
benkatz | 44:8040fa2fcb0d | 51 | DigitalOut drv_cs(PA_4); |
benkatz | 44:8040fa2fcb0d | 52 | //DigitalOut drv_en_gate(PA_11); |
benkatz | 44:8040fa2fcb0d | 53 | DRV832x drv(&drv_spi, &drv_cs); |
benkatz | 8:10ae7bc88d6e | 54 | |
benkatz | 26:2b865c00d7e9 | 55 | PositionSensorAM5147 spi(16384, 0.0, NPP); |
benkatz | 20:bf9ea5125d52 | 56 | |
qiudehua | 55:165927e4f815 | 57 | volatile int counts = 0; |
benkatz | 23:2adf23ee0305 | 58 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 59 | volatile int state_change; |
benkatz | 20:bf9ea5125d52 | 60 | |
qiudehua | 55:165927e4f815 | 61 | |
qiudehua | 55:165927e4f815 | 62 | volatile int cal_counts = 0; |
qiudehua | 55:165927e4f815 | 63 | |
benkatz | 26:2b865c00d7e9 | 64 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 65 | //msgAvailable = true; |
benkatz | 53:e85efce8c1eb | 66 | //printf("%d\n\r", rxMsg.id); |
benkatz | 26:2b865c00d7e9 | 67 | can.read(rxMsg); |
benkatz | 28:8c7e29f719c5 | 68 | if((rxMsg.id == CAN_ID)){ |
benkatz | 28:8c7e29f719c5 | 69 | controller.timeout = 0; |
benkatz | 28:8c7e29f719c5 | 70 | 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 | 71 | state = MOTOR_MODE; |
benkatz | 28:8c7e29f719c5 | 72 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 73 | } |
benkatz | 28:8c7e29f719c5 | 74 | 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 | 75 | state = REST_MODE; |
benkatz | 28:8c7e29f719c5 | 76 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 77 | gpio.led->write(0);; |
benkatz | 28:8c7e29f719c5 | 78 | } |
benkatz | 28:8c7e29f719c5 | 79 | 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 | 80 | spi.ZeroPosition(); |
qiudehua | 55:165927e4f815 | 81 | } |
qiudehua | 55:165927e4f815 | 82 | 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))){ |
qiudehua | 55:165927e4f815 | 83 | //Null instruction, which can be used to return data |
benkatz | 28:8c7e29f719c5 | 84 | } |
benkatz | 28:8c7e29f719c5 | 85 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 86 | unpack_cmd(rxMsg, &controller); |
benkatz | 28:8c7e29f719c5 | 87 | } |
qiudehua | 55:165927e4f815 | 88 | //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); |
qiudehua | 55:165927e4f815 | 89 | //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT); // modify by 20210617 |
qiudehua | 55:165927e4f815 | 90 | //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT); // 正式使用 plan A&C, modify by 20210630 |
qiudehua | 55:165927e4f815 | 91 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT); // test v?, modify by 20210706 |
benkatz | 37:c0f352d6e8e3 | 92 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 93 | } |
benkatz | 26:2b865c00d7e9 | 94 | |
benkatz | 26:2b865c00d7e9 | 95 | } |
benkatz | 26:2b865c00d7e9 | 96 | |
benkatz | 23:2adf23ee0305 | 97 | void enter_menu_state(void){ |
benkatz | 44:8040fa2fcb0d | 98 | drv.disable_gd(); |
benkatz | 47:e1196a851f76 | 99 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 100 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 101 | printf(" Commands:\n\r"); |
benkatz | 44:8040fa2fcb0d | 102 | wait_us(10); |
benkatz | 26:2b865c00d7e9 | 103 | printf(" m - Motor Mode\n\r"); |
benkatz | 44:8040fa2fcb0d | 104 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 105 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 106 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 107 | printf(" s - Setup\n\r"); |
benkatz | 44:8040fa2fcb0d | 108 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 109 | printf(" e - Display Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 110 | wait_us(10); |
benkatz | 37:c0f352d6e8e3 | 111 | printf(" z - Set Zero Position\n\r"); |
benkatz | 44:8040fa2fcb0d | 112 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 113 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 44:8040fa2fcb0d | 114 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 115 | state_change = 0; |
benkatz | 37:c0f352d6e8e3 | 116 | gpio.led->write(0); |
benkatz | 23:2adf23ee0305 | 117 | } |
benkatz | 24:58c2d7571207 | 118 | |
benkatz | 24:58c2d7571207 | 119 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 120 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 44:8040fa2fcb0d | 121 | wait_us(10); |
benkatz | 51:6cd89bd6fcaa | 122 | printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:8040fa2fcb0d | 123 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 124 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 44:8040fa2fcb0d | 125 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 126 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 44:8040fa2fcb0d | 127 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 128 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
benkatz | 44:8040fa2fcb0d | 129 | wait_us(10); |
benkatz | 51:6cd89bd6fcaa | 130 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX); |
benkatz | 51:6cd89bd6fcaa | 131 | wait_us(10); |
benkatz | 51:6cd89bd6fcaa | 132 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX); |
benkatz | 44:8040fa2fcb0d | 133 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 134 | printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
benkatz | 44:8040fa2fcb0d | 135 | wait_us(10); |
benkatz | 24:58c2d7571207 | 136 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 44:8040fa2fcb0d | 137 | wait_us(10); |
benkatz | 24:58c2d7571207 | 138 | state_change = 0; |
benkatz | 24:58c2d7571207 | 139 | } |
benkatz | 22:60276ba87ac6 | 140 | |
benkatz | 23:2adf23ee0305 | 141 | void enter_torque_mode(void){ |
benkatz | 44:8040fa2fcb0d | 142 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 143 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 144 | controller.ovp_flag = 0; |
benkatz | 28:8c7e29f719c5 | 145 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 146 | wait(.001); |
benkatz | 23:2adf23ee0305 | 147 | controller.i_d_ref = 0; |
benkatz | 50:ba72df25d10f | 148 | controller.i_q_ref = 0; // Current Setpoints |
benkatz | 37:c0f352d6e8e3 | 149 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 150 | state_change = 0; |
benkatz | 28:8c7e29f719c5 | 151 | printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 152 | } |
benkatz | 22:60276ba87ac6 | 153 | |
benkatz | 23:2adf23ee0305 | 154 | void calibrate(void){ |
benkatz | 44:8040fa2fcb0d | 155 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 156 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 157 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 158 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 159 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
benkatz | 37:c0f352d6e8e3 | 160 | gpio.led->write(0);; // Turn off status LED |
benkatz | 23:2adf23ee0305 | 161 | wait(.2); |
benkatz | 23:2adf23ee0305 | 162 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 44:8040fa2fcb0d | 163 | drv.disable_gd(); |
benkatz | 47:e1196a851f76 | 164 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 165 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 166 | } |
benkatz | 23:2adf23ee0305 | 167 | |
benkatz | 23:2adf23ee0305 | 168 | void print_encoder(void){ |
benkatz | 48:74a40481740c | 169 | printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
benkatz | 48:74a40481740c | 170 | //printf("%d\n\r", spi.GetRawPosition()); |
benkatz | 47:e1196a851f76 | 171 | wait(.001); |
benkatz | 22:60276ba87ac6 | 172 | } |
benkatz | 20:bf9ea5125d52 | 173 | |
benkatz | 23:2adf23ee0305 | 174 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 175 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
benkatz | 2:8724412ad628 | 176 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 177 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 23:2adf23ee0305 | 178 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 179 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 180 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 181 | //for (delay = 0; delay < 55; delay++); |
benkatz | 45:26801179208e | 182 | |
benkatz | 47:e1196a851f76 | 183 | spi.Sample(DT); // sample position sensor |
benkatz | 37:c0f352d6e8e3 | 184 | controller.adc2_raw = ADC2->DR; // Read ADC Data Registers |
benkatz | 23:2adf23ee0305 | 185 | controller.adc1_raw = ADC1->DR; |
benkatz | 37:c0f352d6e8e3 | 186 | controller.adc3_raw = ADC3->DR; |
benkatz | 37:c0f352d6e8e3 | 187 | controller.theta_elec = spi.GetElecPosition(); |
qiudehua | 55:165927e4f815 | 188 | //controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); |
qiudehua | 55:165927e4f815 | 189 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); |
qiudehua | 55:165927e4f815 | 190 | controller.theta_mech = (1.0f)*spi.GetMechPosition(); // modified on 20210617 |
qiudehua | 55:165927e4f815 | 191 | //controller.dtheta_mech = (1.0f)*spi.GetMechVelocity(); // modified on 20210618 |
qiudehua | 55:165927e4f815 | 192 | |
benkatz | 37:c0f352d6e8e3 | 193 | controller.dtheta_elec = spi.GetElecVelocity(); |
benkatz | 51:6cd89bd6fcaa | 194 | controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement |
benkatz | 23:2adf23ee0305 | 195 | /// |
benkatz | 20:bf9ea5125d52 | 196 | |
benkatz | 23:2adf23ee0305 | 197 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 198 | switch(state){ |
benkatz | 37:c0f352d6e8e3 | 199 | case REST_MODE: // Do nothing |
benkatz | 23:2adf23ee0305 | 200 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 201 | enter_menu_state(); |
benkatz | 23:2adf23ee0305 | 202 | } |
benkatz | 23:2adf23ee0305 | 203 | break; |
benkatz | 22:60276ba87ac6 | 204 | |
benkatz | 23:2adf23ee0305 | 205 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 206 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 207 | calibrate(); |
benkatz | 23:2adf23ee0305 | 208 | } |
benkatz | 23:2adf23ee0305 | 209 | break; |
benkatz | 23:2adf23ee0305 | 210 | |
benkatz | 26:2b865c00d7e9 | 211 | case MOTOR_MODE: // Run torque control |
benkatz | 25:f5741040c4bb | 212 | if(state_change){ |
qiudehua | 55:165927e4f815 | 213 | |
benkatz | 25:f5741040c4bb | 214 | enter_torque_mode(); |
qiudehua | 55:165927e4f815 | 215 | |
qiudehua | 55:165927e4f815 | 216 | counts = 0; //20210222 |
qiudehua | 55:165927e4f815 | 217 | |
benkatz | 25:f5741040c4bb | 218 | } |
benkatz | 28:8c7e29f719c5 | 219 | else{ |
benkatz | 37:c0f352d6e8e3 | 220 | /* |
benkatz | 37:c0f352d6e8e3 | 221 | 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 | 222 | gpio. |
benkatz | 47:e1196a851f76 | 223 | ->write(0); |
benkatz | 37:c0f352d6e8e3 | 224 | controller.ovp_flag = 1; |
benkatz | 37:c0f352d6e8e3 | 225 | state = REST_MODE; |
benkatz | 37:c0f352d6e8e3 | 226 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 227 | printf("OVP Triggered!\n\r"); |
benkatz | 37:c0f352d6e8e3 | 228 | } |
benkatz | 37:c0f352d6e8e3 | 229 | */ |
benkatz | 37:c0f352d6e8e3 | 230 | |
benkatz | 28:8c7e29f719c5 | 231 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 232 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 233 | controller.i_q_ref = 0; |
benkatz | 37:c0f352d6e8e3 | 234 | controller.kp = 0; |
benkatz | 37:c0f352d6e8e3 | 235 | controller.kd = 0; |
benkatz | 37:c0f352d6e8e3 | 236 | controller.t_ff = 0; |
benkatz | 28:8c7e29f719c5 | 237 | } |
benkatz | 47:e1196a851f76 | 238 | |
benkatz | 50:ba72df25d10f | 239 | torque_control(&controller); |
benkatz | 49:83d83040ea51 | 240 | commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop |
benkatz | 49:83d83040ea51 | 241 | controller.timeout++; |
qiudehua | 55:165927e4f815 | 242 | counts ++; |
benkatz | 37:c0f352d6e8e3 | 243 | |
benkatz | 37:c0f352d6e8e3 | 244 | } |
benkatz | 23:2adf23ee0305 | 245 | break; |
benkatz | 23:2adf23ee0305 | 246 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 247 | if(state_change){ |
benkatz | 24:58c2d7571207 | 248 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 249 | } |
benkatz | 23:2adf23ee0305 | 250 | break; |
benkatz | 23:2adf23ee0305 | 251 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 252 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 253 | break; |
qiudehua | 55:165927e4f815 | 254 | } |
benkatz | 2:8724412ad628 | 255 | } |
benkatz | 23:2adf23ee0305 | 256 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 257 | } |
benkatz | 0:4e1c4df6aabd | 258 | |
benkatz | 25:f5741040c4bb | 259 | |
benkatz | 24:58c2d7571207 | 260 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 261 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 262 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 263 | |
benkatz | 25:f5741040c4bb | 264 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 265 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 266 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 267 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 268 | char c = pc.getc(); |
benkatz | 25:f5741040c4bb | 269 | if(c == 27){ |
benkatz | 25:f5741040c4bb | 270 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 271 | state_change = 1; |
benkatz | 25:f5741040c4bb | 272 | char_count = 0; |
benkatz | 25:f5741040c4bb | 273 | cmd_id = 0; |
benkatz | 37:c0f352d6e8e3 | 274 | gpio.led->write(0);; |
benkatz | 25:f5741040c4bb | 275 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 276 | } |
benkatz | 24:58c2d7571207 | 277 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 278 | switch (c){ |
benkatz | 23:2adf23ee0305 | 279 | case 'c': |
benkatz | 23:2adf23ee0305 | 280 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 281 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 282 | break; |
benkatz | 26:2b865c00d7e9 | 283 | case 'm': |
benkatz | 26:2b865c00d7e9 | 284 | state = MOTOR_MODE; |
qiudehua | 55:165927e4f815 | 285 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 286 | break; |
benkatz | 23:2adf23ee0305 | 287 | case 'e': |
benkatz | 23:2adf23ee0305 | 288 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 289 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 290 | break; |
benkatz | 23:2adf23ee0305 | 291 | case 's': |
benkatz | 23:2adf23ee0305 | 292 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 293 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 294 | break; |
benkatz | 37:c0f352d6e8e3 | 295 | case 'z': |
benkatz | 37:c0f352d6e8e3 | 296 | spi.SetMechOffset(0); |
benkatz | 47:e1196a851f76 | 297 | spi.Sample(DT); |
benkatz | 37:c0f352d6e8e3 | 298 | wait_us(20); |
benkatz | 37:c0f352d6e8e3 | 299 | M_OFFSET = spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 300 | if (!prefs.ready()) prefs.open(); |
benkatz | 37:c0f352d6e8e3 | 301 | prefs.flush(); // Write new prefs to flash |
benkatz | 37:c0f352d6e8e3 | 302 | prefs.close(); |
benkatz | 37:c0f352d6e8e3 | 303 | prefs.load(); |
benkatz | 37:c0f352d6e8e3 | 304 | spi.SetMechOffset(M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 305 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 306 | |
benkatz | 37:c0f352d6e8e3 | 307 | break; |
benkatz | 37:c0f352d6e8e3 | 308 | } |
benkatz | 37:c0f352d6e8e3 | 309 | |
benkatz | 24:58c2d7571207 | 310 | } |
benkatz | 24:58c2d7571207 | 311 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 312 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 313 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 314 | case 'b': |
benkatz | 24:58c2d7571207 | 315 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 316 | break; |
benkatz | 24:58c2d7571207 | 317 | case 'i': |
benkatz | 24:58c2d7571207 | 318 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 319 | break; |
benkatz | 26:2b865c00d7e9 | 320 | case 'm': |
benkatz | 26:2b865c00d7e9 | 321 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 322 | break; |
benkatz | 24:58c2d7571207 | 323 | case 'l': |
benkatz | 51:6cd89bd6fcaa | 324 | I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f); |
benkatz | 51:6cd89bd6fcaa | 325 | break; |
benkatz | 51:6cd89bd6fcaa | 326 | case 'f': |
benkatz | 51:6cd89bd6fcaa | 327 | I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 328 | break; |
benkatz | 28:8c7e29f719c5 | 329 | case 't': |
benkatz | 28:8c7e29f719c5 | 330 | CAN_TIMEOUT = atoi(cmd_val); |
benkatz | 28:8c7e29f719c5 | 331 | break; |
benkatz | 24:58c2d7571207 | 332 | default: |
benkatz | 24:58c2d7571207 | 333 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 334 | break; |
benkatz | 24:58c2d7571207 | 335 | } |
benkatz | 24:58c2d7571207 | 336 | |
benkatz | 24:58c2d7571207 | 337 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 338 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 339 | prefs.close(); |
benkatz | 24:58c2d7571207 | 340 | prefs.load(); |
benkatz | 24:58c2d7571207 | 341 | state_change = 1; |
benkatz | 24:58c2d7571207 | 342 | char_count = 0; |
benkatz | 24:58c2d7571207 | 343 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 344 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 345 | } |
benkatz | 24:58c2d7571207 | 346 | else{ |
benkatz | 24:58c2d7571207 | 347 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 348 | else{ |
benkatz | 24:58c2d7571207 | 349 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 350 | |
benkatz | 24:58c2d7571207 | 351 | } |
benkatz | 24:58c2d7571207 | 352 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 353 | char_count++; |
benkatz | 23:2adf23ee0305 | 354 | } |
benkatz | 23:2adf23ee0305 | 355 | } |
benkatz | 24:58c2d7571207 | 356 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 357 | switch (c){ |
benkatz | 24:58c2d7571207 | 358 | case 27: |
benkatz | 24:58c2d7571207 | 359 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 360 | state_change = 1; |
benkatz | 24:58c2d7571207 | 361 | break; |
benkatz | 24:58c2d7571207 | 362 | } |
benkatz | 24:58c2d7571207 | 363 | } |
benkatz | 49:83d83040ea51 | 364 | else if (state == MOTOR_MODE){ |
benkatz | 49:83d83040ea51 | 365 | switch (c){ |
benkatz | 49:83d83040ea51 | 366 | case 'd': |
benkatz | 49:83d83040ea51 | 367 | controller.i_q_ref = 0; |
benkatz | 49:83d83040ea51 | 368 | controller.i_d_ref = 0; |
benkatz | 49:83d83040ea51 | 369 | } |
benkatz | 49:83d83040ea51 | 370 | } |
benkatz | 24:58c2d7571207 | 371 | |
benkatz | 24:58c2d7571207 | 372 | } |
benkatz | 22:60276ba87ac6 | 373 | } |
qiudehua | 55:165927e4f815 | 374 | |
qiudehua | 55:165927e4f815 | 375 | //PT1000用二分法查表计算温度 |
qiudehua | 55:165927e4f815 | 376 | //测温范围:-50~150℃ |
qiudehua | 55:165927e4f815 | 377 | float RTD_TAB_PT1000[201] = { |
qiudehua | 55:165927e4f815 | 378 | |
qiudehua | 55:165927e4f815 | 379 | 803.08,807.05,811.02,814.98,818.95,822.92,826.88,830.84,834.80,838.76, //-50~-41 |
qiudehua | 55:165927e4f815 | 380 | 842.72,846.68,850.63,854.58,858.54,862.49,866.44,870.39,874.33,878.28, //-40~-31 |
qiudehua | 55:165927e4f815 | 381 | 882.23,886.17,890.11,894.05,897.99,901.93,905.87,909.80,913.74,917.67, //-30~-21 |
qiudehua | 55:165927e4f815 | 382 | 921.60,925.54,929.47,933.39,937.32,941.25,945.17,949.10,953.02,956.94, //-20~-11 |
qiudehua | 55:165927e4f815 | 383 | 960.86,964.78,968.70,972.62,976.53,980.45,984.36,988.27,992.18,996.09, //-10~-1 |
qiudehua | 55:165927e4f815 | 384 | 1000.00,1003.91,1007.81,1011.72,1015.62,1019.53,1023.43,1027.33,1031.23,1035.13, //0~9 |
qiudehua | 55:165927e4f815 | 385 | 1039.02,1042.92,1046.81,1050.71,1054.60,1058.49,1062.38,1066.27,1070.16,1074.04, //10~19 |
qiudehua | 55:165927e4f815 | 386 | 1077.93,1081.81,1085.70,1089.58,1093.46,1097.34,1101.22,1105.10,1108.97,1112.85, //20~29 |
qiudehua | 55:165927e4f815 | 387 | 1116.72,1120.59,1124.46,1128.34,1132.20,1136.07,1139.94,1143.81,1147.67,1151.53, //30~39 |
qiudehua | 55:165927e4f815 | 388 | 1155.40,1159.26,1163.12,1166.98,1170.83,1174.69,1178.55,1182.40,1186.25,1190.11, //40~49 |
qiudehua | 55:165927e4f815 | 389 | 1193.96,1197.81,1201.65,1205.50,1209.35,1213.19,1217.04,1220.88,1224.72,1228.56, //50~59 |
qiudehua | 55:165927e4f815 | 390 | 1232.40,1236.24,1240.08,1243.91,1247.75,1251.58,1255.41,1259.24,1263.07,1266.90, //60~69 |
qiudehua | 55:165927e4f815 | 391 | 1270.73,1274.56,1278.38,1282.21,1286.03,1289.85,1293.67,1297.49,1301.31,1305.13, //70~79 |
qiudehua | 55:165927e4f815 | 392 | 1308.94,1312.76,1316.57,1320.39,1324.20,1328.01,1331.82,1335.62,1339.43,1343.24, //80~89 |
qiudehua | 55:165927e4f815 | 393 | 1347.04,1350.85,1354.65,1358.45,1362.25,1366.05,1369.85,1373.64,1377.44,1381.23, //90~99 |
qiudehua | 55:165927e4f815 | 394 | 1385.03,1388.82,1392.61,1396.40,1400.19,1403.97,1407.76,1411.54,1415.33,1419.11, //100~109 |
qiudehua | 55:165927e4f815 | 395 | 1422.89,1426.67,1430.45,1434.23,1438.01,1441.78,1445.56,1449.33,1453.10,1456.87, //110~119 |
qiudehua | 55:165927e4f815 | 396 | 1460.64,1464.41,1468.18,1471.95,1475.71,1479.48,1483.24,1487.00,1490.76,1494.52, //120~129 |
qiudehua | 55:165927e4f815 | 397 | 1498.28,1502.04,1505.79,1509.55,1513.30,1517.06,1520.81,1524.56,1528.31,1532.05, //130~139 |
qiudehua | 55:165927e4f815 | 398 | 1535.80,1539.55,1543.29,1547.03,1550.78,1554.52,1558.26,1562.00,1565.73,1569.47, //140~149 |
qiudehua | 55:165927e4f815 | 399 | 1573.21 //150 |
qiudehua | 55:165927e4f815 | 400 | }; |
qiudehua | 55:165927e4f815 | 401 | |
qiudehua | 55:165927e4f815 | 402 | /******************************************** |
qiudehua | 55:165927e4f815 | 403 | 功能:采用2分查找法,根据电阻值查表计算温度值 |
qiudehua | 55:165927e4f815 | 404 | 输入:fR,PT1000的电阻值 |
qiudehua | 55:165927e4f815 | 405 | 输出:fTemp,测量的温度值 |
qiudehua | 55:165927e4f815 | 406 | ********************************************/ |
qiudehua | 55:165927e4f815 | 407 | float CalTemp(float fR) |
qiudehua | 55:165927e4f815 | 408 | { |
qiudehua | 55:165927e4f815 | 409 | float fTemp; |
qiudehua | 55:165927e4f815 | 410 | float fLowR; |
qiudehua | 55:165927e4f815 | 411 | float fHighR; |
qiudehua | 55:165927e4f815 | 412 | int iTemp; |
qiudehua | 55:165927e4f815 | 413 | int i; |
qiudehua | 55:165927e4f815 | 414 | int Top,Bottom; |
qiudehua | 55:165927e4f815 | 415 | |
qiudehua | 55:165927e4f815 | 416 | cal_counts=0; |
qiudehua | 55:165927e4f815 | 417 | |
qiudehua | 55:165927e4f815 | 418 | if(fR < RTD_TAB_PT1000[0]) //电阻值小于表格最小值 |
qiudehua | 55:165927e4f815 | 419 | { |
qiudehua | 55:165927e4f815 | 420 | return -50; |
qiudehua | 55:165927e4f815 | 421 | } |
qiudehua | 55:165927e4f815 | 422 | |
qiudehua | 55:165927e4f815 | 423 | if(fR > RTD_TAB_PT1000[200]) //电阻值大于表格最大值 |
qiudehua | 55:165927e4f815 | 424 | { |
qiudehua | 55:165927e4f815 | 425 | return 150; |
qiudehua | 55:165927e4f815 | 426 | } |
qiudehua | 55:165927e4f815 | 427 | |
qiudehua | 55:165927e4f815 | 428 | Bottom = 0; |
qiudehua | 55:165927e4f815 | 429 | Top = 200; |
qiudehua | 55:165927e4f815 | 430 | |
qiudehua | 55:165927e4f815 | 431 | for(i=100;(Top-Bottom)!=1;) //2分法查表 |
qiudehua | 55:165927e4f815 | 432 | { |
qiudehua | 55:165927e4f815 | 433 | if(fR < RTD_TAB_PT1000[i]) |
qiudehua | 55:165927e4f815 | 434 | { |
qiudehua | 55:165927e4f815 | 435 | Top = i; |
qiudehua | 55:165927e4f815 | 436 | i = (Top + Bottom) / 2; |
qiudehua | 55:165927e4f815 | 437 | } |
qiudehua | 55:165927e4f815 | 438 | else if(fR > RTD_TAB_PT1000[i]) |
qiudehua | 55:165927e4f815 | 439 | { |
qiudehua | 55:165927e4f815 | 440 | Bottom = i; |
qiudehua | 55:165927e4f815 | 441 | i = (Top + Bottom) / 2; |
qiudehua | 55:165927e4f815 | 442 | } |
qiudehua | 55:165927e4f815 | 443 | else |
qiudehua | 55:165927e4f815 | 444 | { |
qiudehua | 55:165927e4f815 | 445 | iTemp = i - 50; |
qiudehua | 55:165927e4f815 | 446 | fTemp = (float)iTemp; |
qiudehua | 55:165927e4f815 | 447 | return fTemp; |
qiudehua | 55:165927e4f815 | 448 | } |
qiudehua | 55:165927e4f815 | 449 | cal_counts++; |
qiudehua | 55:165927e4f815 | 450 | } |
qiudehua | 55:165927e4f815 | 451 | |
qiudehua | 55:165927e4f815 | 452 | iTemp = i - 50; |
qiudehua | 55:165927e4f815 | 453 | fLowR = RTD_TAB_PT1000[Bottom]; |
qiudehua | 55:165927e4f815 | 454 | fHighR = RTD_TAB_PT1000[Top]; |
qiudehua | 55:165927e4f815 | 455 | fTemp = (fR - fLowR)/(fHighR - fLowR) + iTemp; |
qiudehua | 55:165927e4f815 | 456 | return fTemp; |
qiudehua | 55:165927e4f815 | 457 | } |
qiudehua | 55:165927e4f815 | 458 | |
qiudehua | 55:165927e4f815 | 459 | |
qiudehua | 55:165927e4f815 | 460 | |
qiudehua | 55:165927e4f815 | 461 | |
qiudehua | 55:165927e4f815 | 462 | |
benkatz | 0:4e1c4df6aabd | 463 | |
benkatz | 0:4e1c4df6aabd | 464 | int main() { |
qiudehua | 55:165927e4f815 | 465 | //************************* |
qiudehua | 55:165927e4f815 | 466 | //uint32_t btr; |
qiudehua | 55:165927e4f815 | 467 | //CAN_TypeDef *can = obj->CanHandle.Instance; |
qiudehua | 55:165927e4f815 | 468 | /* |
qiudehua | 55:165927e4f815 | 469 | hcan->Instance->BTR = (uint32_t)((uint32_t)hcan->Init.Mode) | \ |
qiudehua | 55:165927e4f815 | 470 | ((uint32_t)hcan->Init.SJW) | \ |
qiudehua | 55:165927e4f815 | 471 | ((uint32_t)hcan->Init.BS1) | \ |
qiudehua | 55:165927e4f815 | 472 | ((uint32_t)hcan->Init.BS2) | \ |
qiudehua | 55:165927e4f815 | 473 | ((uint32_t)hcan->Init.Prescaler - 1U); |
qiudehua | 55:165927e4f815 | 474 | */ |
qiudehua | 55:165927e4f815 | 475 | |
qiudehua | 55:165927e4f815 | 476 | //uint32_t i,num; |
qiudehua | 55:165927e4f815 | 477 | float ii; |
qiudehua | 55:165927e4f815 | 478 | |
qiudehua | 55:165927e4f815 | 479 | |
benkatz | 20:bf9ea5125d52 | 480 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 481 | controller.mode = 0; |
benkatz | 23:2adf23ee0305 | 482 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 44:8040fa2fcb0d | 483 | wait(.1); |
benkatz | 44:8040fa2fcb0d | 484 | |
benkatz | 44:8040fa2fcb0d | 485 | gpio.enable->write(1); |
benkatz | 44:8040fa2fcb0d | 486 | wait_us(100); |
benkatz | 45:26801179208e | 487 | drv.calibrate(); |
benkatz | 45:26801179208e | 488 | wait_us(100); |
benkatz | 44:8040fa2fcb0d | 489 | drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); |
benkatz | 44:8040fa2fcb0d | 490 | wait_us(100); |
benkatz | 46:2d4b1dafcfe3 | 491 | drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); |
benkatz | 44:8040fa2fcb0d | 492 | wait_us(100); |
benkatz | 49:83d83040ea51 | 493 | drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88); |
benkatz | 45:26801179208e | 494 | |
benkatz | 45:26801179208e | 495 | //drv.enable_gd(); |
benkatz | 44:8040fa2fcb0d | 496 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 47:e1196a851f76 | 497 | drv.disable_gd(); |
benkatz | 20:bf9ea5125d52 | 498 | |
benkatz | 9:d7eb815cb057 | 499 | wait(.1); |
benkatz | 44:8040fa2fcb0d | 500 | /* |
benkatz | 26:2b865c00d7e9 | 501 | gpio.enable->write(1); |
benkatz | 26:2b865c00d7e9 | 502 | TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles |
benkatz | 26:2b865c00d7e9 | 503 | TIM1->CCR2 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 504 | TIM1->CCR1 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 505 | gpio.enable->write(0); |
benkatz | 44:8040fa2fcb0d | 506 | */ |
benkatz | 23:2adf23ee0305 | 507 | reset_foc(&controller); // Reset current controller |
benkatz | 48:74a40481740c | 508 | reset_observer(&observer); // Reset observer |
benkatz | 26:2b865c00d7e9 | 509 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 26:2b865c00d7e9 | 510 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 511 | |
benkatz | 20:bf9ea5125d52 | 512 | wait(.1); |
benkatz | 37:c0f352d6e8e3 | 513 | NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication |
benkatz | 43:dfb72608639c | 514 | |
benkatz | 37:c0f352d6e8e3 | 515 | NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
benkatz | 53:e85efce8c1eb | 516 | // attach 'CAN receive-complete' interrupt handler |
benkatz | 53:e85efce8c1eb | 517 | |
benkatz | 53:e85efce8c1eb | 518 | // If preferences haven't been user configured yet, set defaults |
benkatz | 53:e85efce8c1eb | 519 | prefs.load(); // Read flash |
benkatz | 53:e85efce8c1eb | 520 | |
benkatz | 53:e85efce8c1eb | 521 | can.filter(CAN_ID , 0xFFF, CANStandard, 0); |
benkatz | 43:dfb72608639c | 522 | |
benkatz | 28:8c7e29f719c5 | 523 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 524 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 525 | rxMsg.len = 8; |
benkatz | 53:e85efce8c1eb | 526 | can.attach(&onMsgReceived); |
benkatz | 23:2adf23ee0305 | 527 | |
benkatz | 37:c0f352d6e8e3 | 528 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
benkatz | 37:c0f352d6e8e3 | 529 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
benkatz | 48:74a40481740c | 530 | if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} |
qiudehua | 55:165927e4f815 | 531 | if(isnan(I_MAX) || I_MAX ==-1){I_MAX=30;} |
benkatz | 51:6cd89bd6fcaa | 532 | if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;} |
benkatz | 48:74a40481740c | 533 | if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} |
benkatz | 48:74a40481740c | 534 | if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} |
benkatz | 48:74a40481740c | 535 | if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;} |
benkatz | 25:f5741040c4bb | 536 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 37:c0f352d6e8e3 | 537 | spi.SetMechOffset(M_OFFSET); |
benkatz | 23:2adf23ee0305 | 538 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 539 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 25:f5741040c4bb | 540 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
benkatz | 45:26801179208e | 541 | init_controller_params(&controller); |
benkatz | 45:26801179208e | 542 | |
benkatz | 26:2b865c00d7e9 | 543 | pc.baud(921600); // set serial baud rate |
qiudehua | 55:165927e4f815 | 544 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 20:bf9ea5125d52 | 545 | wait(.01); |
qiudehua | 55:165927e4f815 | 546 | pc.printf("\n\r\n\r GTGJ MOTOR\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 547 | wait(.01); |
benkatz | 23:2adf23ee0305 | 548 | printf("\n\r Debug Info:\n\r"); |
benkatz | 32:ccac5da77844 | 549 | printf(" Firmware Version: %s\n\r", VERSION_NUM); |
benkatz | 23:2adf23ee0305 | 550 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 551 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 552 | printf(" Output Zero Position: %.4f\n\r", M_OFFSET); |
benkatz | 24:58c2d7571207 | 553 | printf(" CAN ID: %d\n\r", CAN_ID); |
benkatz | 44:8040fa2fcb0d | 554 | |
benkatz | 44:8040fa2fcb0d | 555 | |
qiudehua | 55:165927e4f815 | 556 | //********* |
benkatz | 47:e1196a851f76 | 557 | //printf(" %d\n\r", drv.read_register(DCR)); |
benkatz | 47:e1196a851f76 | 558 | //wait_us(100); |
benkatz | 47:e1196a851f76 | 559 | //printf(" %d\n\r", drv.read_register(CSACR)); |
benkatz | 47:e1196a851f76 | 560 | //wait_us(100); |
benkatz | 47:e1196a851f76 | 561 | //printf(" %d\n\r", drv.read_register(OCPCR)); |
benkatz | 47:e1196a851f76 | 562 | //drv.disable_gd(); |
benkatz | 44:8040fa2fcb0d | 563 | |
qiudehua | 55:165927e4f815 | 564 | // pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 22:60276ba87ac6 | 565 | |
benkatz | 23:2adf23ee0305 | 566 | state_change = 1; |
benkatz | 50:ba72df25d10f | 567 | |
benkatz | 20:bf9ea5125d52 | 568 | |
qiudehua | 55:165927e4f815 | 569 | //********************************************************************************************** |
qiudehua | 55:165927e4f815 | 570 | //for(i=800;i<1580;i++) |
qiudehua | 55:165927e4f815 | 571 | for(ii=1600;ii<3150;ii++) |
qiudehua | 55:165927e4f815 | 572 | { |
qiudehua | 55:165927e4f815 | 573 | //printf("%d %d %.2f\n\r", ii/2, cal_counts, CalTemp((float)ii/2)); |
qiudehua | 55:165927e4f815 | 574 | printf("%.1f %d %.2f\n\r", ii/2.0f, cal_counts, CalTemp(ii/2.0f)); |
qiudehua | 55:165927e4f815 | 575 | } |
qiudehua | 55:165927e4f815 | 576 | |
qiudehua | 55:165927e4f815 | 577 | //********************************************************************************************** |
qiudehua | 55:165927e4f815 | 578 | |
qiudehua | 55:165927e4f815 | 579 | // int counter = 0; |
benkatz | 0:4e1c4df6aabd | 580 | while(1) { |
benkatz | 47:e1196a851f76 | 581 | drv.print_faults(); |
benkatz | 53:e85efce8c1eb | 582 | wait(.1); |
qiudehua | 55:165927e4f815 | 583 | //printf("%.4f\n\r", controller.v_bus); |
benkatz | 50:ba72df25d10f | 584 | /* |
benkatz | 47:e1196a851f76 | 585 | if(state == MOTOR_MODE) |
benkatz | 47:e1196a851f76 | 586 | { |
benkatz | 48:74a40481740c | 587 | //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); |
qiudehua | 55:165927e4f815 | 588 | //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); |
qiudehua | 55:165927e4f815 | 589 | //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); |
qiudehua | 55:165927e4f815 | 590 | 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); |
qiudehua | 55:165927e4f815 | 591 | //printf("%.3f\n\r", controller.dtheta_mech); |
qiudehua | 55:165927e4f815 | 592 | //printf("%d %.4f %.4f\n\r", spi.GetRawPosition(), spi.GetMechPositionFixed(),spi.GetMechPosition()); |
qiudehua | 55:165927e4f815 | 593 | wait(.005); |
benkatz | 47:e1196a851f76 | 594 | } |
benkatz | 50:ba72df25d10f | 595 | */ |
benkatz | 47:e1196a851f76 | 596 | |
benkatz | 0:4e1c4df6aabd | 597 | } |
benkatz | 0:4e1c4df6aabd | 598 | } |