Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
main.cpp@78:4f481ff040ee, 21 months ago (annotated)
- Committer:
- adimmit
- Date:
- Fri Nov 11 16:58:43 2022 +0000
- Revision:
- 78:4f481ff040ee
- Parent:
- 77:81c2cbc5d906
changes w/ filter @ high speed;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
adimmit | 77:81c2cbc5d906 | 1 | //SET THESE FOR YOUR MOTOR |
adimmit | 77:81c2cbc5d906 | 2 | #define V_U12_MIN -65.0f |
adimmit | 77:81c2cbc5d906 | 3 | #define V_U12_MAX 65.0f |
adimmit | 77:81c2cbc5d906 | 4 | |
adimmit | 77:81c2cbc5d906 | 5 | |
adimmit | 77:81c2cbc5d906 | 6 | |
adimmit | 77:81c2cbc5d906 | 7 | |
benkatz | 22:60276ba87ac6 | 8 | /// high-bandwidth 3-phase motor control, for robots |
benkatz | 45:26801179208e | 9 | /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others |
benkatz | 22:60276ba87ac6 | 10 | /// Hardware documentation can be found at build-its.blogspot.com |
benkatz | 22:60276ba87ac6 | 11 | /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling |
benkatz | 47:e1196a851f76 | 12 | /// Version for the TI DRV8323 Everything Chip |
benkatz | 22:60276ba87ac6 | 13 | |
benkatz | 55:c4c9fec8539c | 14 | #define REST_MODE 0 |
benkatz | 55:c4c9fec8539c | 15 | #define CALIBRATION_MODE 1 |
benkatz | 55:c4c9fec8539c | 16 | #define MOTOR_MODE 2 |
benkatz | 55:c4c9fec8539c | 17 | #define SETUP_MODE 4 |
benkatz | 55:c4c9fec8539c | 18 | #define ENCODER_MODE 5 |
benkatz | 55:c4c9fec8539c | 19 | #define INIT_TEMP_MODE 6 |
benkatz | 22:60276ba87ac6 | 20 | |
adimmit | 73:5676f1232a81 | 21 | #define VERSION_NUM "2.0" //changed to 2.0 for flash storage version |
benkatz | 26:2b865c00d7e9 | 22 | |
adimmit | 76:4fd876d4cf2b | 23 | #define LOG_POINTS 500 |
benkatz | 18:f1d56f4acb39 | 24 | |
benkatz | 26:2b865c00d7e9 | 25 | float __float_reg[64]; // Floats stored in flash |
benkatz | 26:2b865c00d7e9 | 26 | int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table |
adimmit | 76:4fd876d4cf2b | 27 | float pos_high_frequency[LOG_POINTS]; |
benkatz | 17:3c5df2982199 | 28 | |
benkatz | 0:4e1c4df6aabd | 29 | #include "mbed.h" |
benkatz | 0:4e1c4df6aabd | 30 | #include "PositionSensor.h" |
benkatz | 20:bf9ea5125d52 | 31 | #include "structs.h" |
benkatz | 55:c4c9fec8539c | 32 | #include "foc.h" |
benkatz | 22:60276ba87ac6 | 33 | #include "calibration.h" |
benkatz | 20:bf9ea5125d52 | 34 | #include "hw_setup.h" |
benkatz | 23:2adf23ee0305 | 35 | #include "math_ops.h" |
benkatz | 20:bf9ea5125d52 | 36 | #include "current_controller_config.h" |
benkatz | 20:bf9ea5125d52 | 37 | #include "hw_config.h" |
adimmit | 73:5676f1232a81 | 38 | //#include "motor_config_U12.h" // need to choose between two files here, one for U10 and one for U12 |
benkatz | 23:2adf23ee0305 | 39 | #include "stm32f4xx_flash.h" |
benkatz | 23:2adf23ee0305 | 40 | #include "FlashWriter.h" |
benkatz | 23:2adf23ee0305 | 41 | #include "user_config.h" |
benkatz | 23:2adf23ee0305 | 42 | #include "PreferenceWriter.h" |
benkatz | 42:738fa01b0346 | 43 | #include "CAN_com.h" |
benkatz | 44:8040fa2fcb0d | 44 | #include "DRV.h" |
benkatz | 26:2b865c00d7e9 | 45 | |
benkatz | 23:2adf23ee0305 | 46 | PreferenceWriter prefs(6); |
benkatz | 9:d7eb815cb057 | 47 | |
benkatz | 20:bf9ea5125d52 | 48 | GPIOStruct gpio; |
benkatz | 20:bf9ea5125d52 | 49 | ControllerStruct controller; |
benkatz | 48:74a40481740c | 50 | ObserverStruct observer; |
benkatz | 20:bf9ea5125d52 | 51 | COMStruct com; |
benkatz | 43:dfb72608639c | 52 | Serial pc(PA_2, PA_3); |
benkatz | 9:d7eb815cb057 | 53 | |
benkatz | 17:3c5df2982199 | 54 | |
benkatz | 45:26801179208e | 55 | CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name |
benkatz | 26:2b865c00d7e9 | 56 | CANMessage rxMsg; |
benkatz | 26:2b865c00d7e9 | 57 | CANMessage txMsg; |
benkatz | 23:2adf23ee0305 | 58 | |
benkatz | 20:bf9ea5125d52 | 59 | |
benkatz | 44:8040fa2fcb0d | 60 | SPI drv_spi(PA_7, PA_6, PA_5); |
benkatz | 44:8040fa2fcb0d | 61 | DigitalOut drv_cs(PA_4); |
benkatz | 44:8040fa2fcb0d | 62 | //DigitalOut drv_en_gate(PA_11); |
benkatz | 44:8040fa2fcb0d | 63 | DRV832x drv(&drv_spi, &drv_cs); |
benkatz | 8:10ae7bc88d6e | 64 | |
saloutos | 58:32e8927fe39f | 65 | //PositionSensoriCMU spi(16777216, 0.0, NPP); // Read 24 bits off the sensor |
saloutos | 58:32e8927fe39f | 66 | PositionSensoriCMU spi(524288, 0.0, NPP); // Read 24 bits off the sensor, only 19bits are encoder data |
benkatz | 20:bf9ea5125d52 | 67 | |
benkatz | 23:2adf23ee0305 | 68 | volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 69 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 70 | volatile int state_change; |
adimmit | 77:81c2cbc5d906 | 71 | volatile int logger = 0; |
adimmit | 78:4f481ff040ee | 72 | volatile int counter = 0; |
adimmit | 77:81c2cbc5d906 | 73 | |
adimmit | 77:81c2cbc5d906 | 74 | //Position Sensor Temperature Fix - INITIALIZE THESE IN THE MAIN SETUP |
adimmit | 77:81c2cbc5d906 | 75 | volatile float prev_mech = 0.0; |
adimmit | 77:81c2cbc5d906 | 76 | volatile float prev_elec = 0.0; |
adimmit | 78:4f481ff040ee | 77 | volatile float prev_dmech = 0.0; |
adimmit | 78:4f481ff040ee | 78 | volatile float prev_delec = 0.0; |
adimmit | 77:81c2cbc5d906 | 79 | volatile int failure_counter = 0; |
benkatz | 20:bf9ea5125d52 | 80 | |
benkatz | 26:2b865c00d7e9 | 81 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 82 | //msgAvailable = true; |
benkatz | 53:e85efce8c1eb | 83 | //printf("%d\n\r", rxMsg.id); |
saloutos | 58:32e8927fe39f | 84 | gpio.led->write(1); |
benkatz | 26:2b865c00d7e9 | 85 | can.read(rxMsg); |
benkatz | 28:8c7e29f719c5 | 86 | if((rxMsg.id == CAN_ID)){ |
benkatz | 28:8c7e29f719c5 | 87 | controller.timeout = 0; |
benkatz | 28:8c7e29f719c5 | 88 | 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 | 89 | state = MOTOR_MODE; |
benkatz | 28:8c7e29f719c5 | 90 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 91 | } |
benkatz | 28:8c7e29f719c5 | 92 | 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 | 93 | state = REST_MODE; |
benkatz | 28:8c7e29f719c5 | 94 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 95 | gpio.led->write(0);; |
benkatz | 28:8c7e29f719c5 | 96 | } |
benkatz | 28:8c7e29f719c5 | 97 | 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 | 98 | spi.ZeroPosition(); |
benkatz | 28:8c7e29f719c5 | 99 | } |
benkatz | 28:8c7e29f719c5 | 100 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 101 | unpack_cmd(rxMsg, &controller); |
benkatz | 28:8c7e29f719c5 | 102 | } |
elijahsj | 68:edfdbd0db876 | 103 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); // multiply theta, dtheta_mech by GR here? |
elijahsj | 68:edfdbd0db876 | 104 | //pack_reply(&txMsg, controller.v_q, controller.dtheta_mech, controller.i_q_filt*KT_OUT); |
saloutos | 58:32e8927fe39f | 105 | //pack_reply(&txMsg, 3.14159f, 6.28318f, 1.00f); // test values |
benkatz | 37:c0f352d6e8e3 | 106 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 107 | } |
saloutos | 58:32e8927fe39f | 108 | gpio.led->write(0); |
benkatz | 26:2b865c00d7e9 | 109 | |
benkatz | 26:2b865c00d7e9 | 110 | } |
benkatz | 26:2b865c00d7e9 | 111 | |
benkatz | 23:2adf23ee0305 | 112 | void enter_menu_state(void){ |
benkatz | 44:8040fa2fcb0d | 113 | drv.disable_gd(); |
benkatz | 55:c4c9fec8539c | 114 | reset_foc(&controller); |
benkatz | 47:e1196a851f76 | 115 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 116 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 117 | printf(" Commands:\n\r"); |
benkatz | 44:8040fa2fcb0d | 118 | wait_us(10); |
benkatz | 26:2b865c00d7e9 | 119 | printf(" m - Motor Mode\n\r"); |
benkatz | 44:8040fa2fcb0d | 120 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 121 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 122 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 123 | printf(" s - Setup\n\r"); |
benkatz | 44:8040fa2fcb0d | 124 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 125 | printf(" e - Display Encoder\n\r"); |
benkatz | 44:8040fa2fcb0d | 126 | wait_us(10); |
benkatz | 37:c0f352d6e8e3 | 127 | printf(" z - Set Zero Position\n\r"); |
benkatz | 44:8040fa2fcb0d | 128 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 129 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 44:8040fa2fcb0d | 130 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 131 | state_change = 0; |
benkatz | 37:c0f352d6e8e3 | 132 | gpio.led->write(0); |
benkatz | 23:2adf23ee0305 | 133 | } |
benkatz | 24:58c2d7571207 | 134 | |
benkatz | 24:58c2d7571207 | 135 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 136 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 44:8040fa2fcb0d | 137 | wait_us(10); |
benkatz | 51:6cd89bd6fcaa | 138 | printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:8040fa2fcb0d | 139 | wait_us(10); |
adimmit | 73:5676f1232a81 | 140 | //MOTOR CONTROLLER PARAMS |
adimmit | 73:5676f1232a81 | 141 | printf("MOTOR CONTROLLER PARAMS\n\r"); |
adimmit | 73:5676f1232a81 | 142 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 143 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 44:8040fa2fcb0d | 144 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 145 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 44:8040fa2fcb0d | 146 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 147 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
benkatz | 44:8040fa2fcb0d | 148 | wait_us(10); |
elijahsj | 66:3947255b18c5 | 149 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "65.0", I_MAX); |
benkatz | 51:6cd89bd6fcaa | 150 | wait_us(10); |
benkatz | 51:6cd89bd6fcaa | 151 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX); |
benkatz | 44:8040fa2fcb0d | 152 | wait_us(10); |
benkatz | 28:8c7e29f719c5 | 153 | printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
benkatz | 44:8040fa2fcb0d | 154 | wait_us(10); |
benkatz | 55:c4c9fec8539c | 155 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "h", "Temp Cutoff (C) (0 = none)", "0", "150", TEMP_MAX); |
benkatz | 55:c4c9fec8539c | 156 | wait_us(10); |
benkatz | 55:c4c9fec8539c | 157 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "c", "Continuous Current (A)", "0", "40.0", I_MAX_CONT); |
benkatz | 55:c4c9fec8539c | 158 | wait_us(10); |
adimmit | 73:5676f1232a81 | 159 | //CURRENT CONTROLLER PARAMS |
adimmit | 73:5676f1232a81 | 160 | printf("CURRENT CONTROLLER PARAMS\n\r"); |
adimmit | 73:5676f1232a81 | 161 | wait_us(10); |
adimmit | 72:3163691b0851 | 162 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "s", "Current Controller K_SCALE", "0", "0.1", K_SCALE); //ADDED --> FOR CURRENT CONTROL GAINS (KP) |
adimmit | 71:b314182de37f | 163 | wait_us(10); |
adimmit | 72:3163691b0851 | 164 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "d", "Current Controller KI_D", "0", "1.0", KI_D); //ADDED --> FOR CURRENT CONTROL GAINS (KI D-Axis) |
adimmit | 71:b314182de37f | 165 | wait_us(10); |
adimmit | 72:3163691b0851 | 166 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "q", "Current Controller KI_Q", "0", "1.0", KI_Q); //ADDED --> FOR CURRENT CONTROL GAINS (KI Q-Axis) |
adimmit | 71:b314182de37f | 167 | wait_us(10); |
adimmit | 73:5676f1232a81 | 168 | //MOTOR PARAMS |
adimmit | 73:5676f1232a81 | 169 | printf("MOTOR PARAMS\n\r"); |
adimmit | 73:5676f1232a81 | 170 | wait_us(10); |
adimmit | 73:5676f1232a81 | 171 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "r", "Motor Phase Resistance (ohms)", "0", "1.0", R_PHASE); |
adimmit | 73:5676f1232a81 | 172 | wait_us(10); |
adimmit | 73:5676f1232a81 | 173 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "a", "D-axis inductance (H)", "0", "0.1", L_D); |
adimmit | 73:5676f1232a81 | 174 | wait_us(10); |
adimmit | 73:5676f1232a81 | 175 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "e", "Q-axis inductance (H)", "0", "0.1", L_Q); |
adimmit | 73:5676f1232a81 | 176 | wait_us(10); |
adimmit | 75:c7fbacd92b8f | 177 | //printf(" %-4s %-31s %-5s %-6s %d\n\r", "n", "Number of Pole Pairs (NPP)", "0", "40", NPP); |
adimmit | 75:c7fbacd92b8f | 178 | //wait_us(10); |
adimmit | 73:5676f1232a81 | 179 | //ACTUATOR PARAMS |
adimmit | 73:5676f1232a81 | 180 | printf("ACTUATOR PARAMS\n\r"); |
adimmit | 73:5676f1232a81 | 181 | wait_us(10); |
adimmit | 74:fcc5f8e7f0ef | 182 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "p", "Torque Constance Nm/amp", "0", "1.0", KT); |
adimmit | 73:5676f1232a81 | 183 | wait_us(10); |
adimmit | 73:5676f1232a81 | 184 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "g", "Module Gear Ratio", "0", "20.0", GR); |
adimmit | 73:5676f1232a81 | 185 | wait_us(10); |
adimmit | 73:5676f1232a81 | 186 | printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "k", "KT*GR", "0", "20.0", KT_OUT); |
adimmit | 73:5676f1232a81 | 187 | wait_us(10); |
adimmit | 74:fcc5f8e7f0ef | 188 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "w", "KT = WB*NPP*3/2", "0", "1.0", WB); |
adimmit | 73:5676f1232a81 | 189 | wait_us(10); |
adimmit | 73:5676f1232a81 | 190 | //THERMAL PARAMS |
adimmit | 73:5676f1232a81 | 191 | printf("THERMAL PARAMS\n\r"); |
adimmit | 73:5676f1232a81 | 192 | wait_us(10); |
adimmit | 73:5676f1232a81 | 193 | printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "x", "Thermal Resistance (K-W/J)", "0", "10.0", R_TH); |
adimmit | 73:5676f1232a81 | 194 | wait_us(10); |
adimmit | 73:5676f1232a81 | 195 | printf(" %-4s %-31s %-5s %-6s %f\n\r", "y", "Observer M Matrix (K/J)", "0", "1.0", INV_M_TH); |
adimmit | 73:5676f1232a81 | 196 | wait_us(10); |
adimmit | 73:5676f1232a81 | 197 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "z", "Ambient Temp @ Calibration (C)", "0", "35.0", T_AMBIENT); |
adimmit | 73:5676f1232a81 | 198 | wait_us(10); |
adimmit | 73:5676f1232a81 | 199 | //END CONFIGS |
benkatz | 24:58c2d7571207 | 200 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 44:8040fa2fcb0d | 201 | wait_us(10); |
benkatz | 24:58c2d7571207 | 202 | state_change = 0; |
benkatz | 24:58c2d7571207 | 203 | } |
benkatz | 22:60276ba87ac6 | 204 | |
benkatz | 23:2adf23ee0305 | 205 | void enter_torque_mode(void){ |
benkatz | 44:8040fa2fcb0d | 206 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 207 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 208 | controller.ovp_flag = 0; |
benkatz | 28:8c7e29f719c5 | 209 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 210 | wait(.001); |
benkatz | 23:2adf23ee0305 | 211 | controller.i_d_ref = 0; |
benkatz | 50:ba72df25d10f | 212 | controller.i_q_ref = 0; // Current Setpoints |
benkatz | 37:c0f352d6e8e3 | 213 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 214 | state_change = 0; |
benkatz | 28:8c7e29f719c5 | 215 | printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 216 | } |
benkatz | 22:60276ba87ac6 | 217 | |
benkatz | 23:2adf23ee0305 | 218 | void calibrate(void){ |
benkatz | 44:8040fa2fcb0d | 219 | drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 220 | //gpio.enable->write(1); |
benkatz | 37:c0f352d6e8e3 | 221 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 222 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 223 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
benkatz | 37:c0f352d6e8e3 | 224 | gpio.led->write(0);; // Turn off status LED |
benkatz | 55:c4c9fec8539c | 225 | wait(.05); |
benkatz | 55:c4c9fec8539c | 226 | R_NOMINAL = 0; |
benkatz | 55:c4c9fec8539c | 227 | state = INIT_TEMP_MODE; |
saloutos | 58:32e8927fe39f | 228 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 55:c4c9fec8539c | 229 | //drv.disable_gd(); |
saloutos | 58:32e8927fe39f | 230 | state_change = 0; |
benkatz | 55:c4c9fec8539c | 231 | |
benkatz | 23:2adf23ee0305 | 232 | } |
benkatz | 23:2adf23ee0305 | 233 | |
benkatz | 23:2adf23ee0305 | 234 | void print_encoder(void){ |
benkatz | 48:74a40481740c | 235 | printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
benkatz | 48:74a40481740c | 236 | //printf("%d\n\r", spi.GetRawPosition()); |
benkatz | 47:e1196a851f76 | 237 | wait(.001); |
benkatz | 22:60276ba87ac6 | 238 | } |
benkatz | 20:bf9ea5125d52 | 239 | |
benkatz | 23:2adf23ee0305 | 240 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 241 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
benkatz | 55:c4c9fec8539c | 242 | //float testing[1000]; |
benkatz | 55:c4c9fec8539c | 243 | //float testing2[1000]; |
benkatz | 2:8724412ad628 | 244 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 245 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 55:c4c9fec8539c | 246 | //gpio.led->write(1); |
benkatz | 23:2adf23ee0305 | 247 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 248 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 249 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 250 | //for (delay = 0; delay < 55; delay++); |
benkatz | 55:c4c9fec8539c | 251 | |
benkatz | 47:e1196a851f76 | 252 | spi.Sample(DT); // sample position sensor |
benkatz | 56:fe5056ac6740 | 253 | /* |
benkatz | 56:fe5056ac6740 | 254 | if(count < 10){printf("%d\n\r", spi.GetRawPosition());} |
benkatz | 56:fe5056ac6740 | 255 | count ++; |
adimmit | 77:81c2cbc5d906 | 256 | */ |
adimmit | 77:81c2cbc5d906 | 257 | |
adimmit | 77:81c2cbc5d906 | 258 | //FILTER |
adimmit | 77:81c2cbc5d906 | 259 | float velocity = (1.0f/GR)*spi.GetMechVelocity(); |
adimmit | 77:81c2cbc5d906 | 260 | |
benkatz | 37:c0f352d6e8e3 | 261 | controller.adc2_raw = ADC2->DR; // Read ADC Data Registers |
benkatz | 23:2adf23ee0305 | 262 | controller.adc1_raw = ADC1->DR; |
benkatz | 37:c0f352d6e8e3 | 263 | controller.adc3_raw = ADC3->DR; |
adimmit | 77:81c2cbc5d906 | 264 | |
adimmit | 78:4f481ff040ee | 265 | if (velocity > V_U12_MAX || velocity < V_U12_MIN) { |
adimmit | 78:4f481ff040ee | 266 | controller.dtheta_mech = 0.0; //previous velocity? |
adimmit | 77:81c2cbc5d906 | 267 | controller.dtheta_elec = 0.0; |
adimmit | 77:81c2cbc5d906 | 268 | controller.theta_elec = prev_elec; |
adimmit | 77:81c2cbc5d906 | 269 | controller.theta_mech = prev_mech; |
adimmit | 77:81c2cbc5d906 | 270 | failure_counter++; |
adimmit | 78:4f481ff040ee | 271 | //pc.printf("FAILURE\n\r"); |
adimmit | 77:81c2cbc5d906 | 272 | } else { |
adimmit | 77:81c2cbc5d906 | 273 | controller.theta_elec = spi.GetElecPosition(); |
adimmit | 77:81c2cbc5d906 | 274 | controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); // should these be divided by gear ratio??? |
adimmit | 77:81c2cbc5d906 | 275 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); // mech pos isn't pre-multiplied by GR in position sensor function... |
adimmit | 77:81c2cbc5d906 | 276 | controller.dtheta_elec = spi.GetElecVelocity(); |
adimmit | 77:81c2cbc5d906 | 277 | prev_elec = controller.theta_elec; |
adimmit | 77:81c2cbc5d906 | 278 | prev_mech = controller.theta_mech; |
adimmit | 78:4f481ff040ee | 279 | prev_dmech = controller.dtheta_mech; |
adimmit | 78:4f481ff040ee | 280 | prev_delec = controller.dtheta_elec; |
adimmit | 77:81c2cbc5d906 | 281 | } |
adimmit | 77:81c2cbc5d906 | 282 | |
benkatz | 51:6cd89bd6fcaa | 283 | controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement |
benkatz | 55:c4c9fec8539c | 284 | |
benkatz | 55:c4c9fec8539c | 285 | |
benkatz | 55:c4c9fec8539c | 286 | |
benkatz | 23:2adf23ee0305 | 287 | /// |
benkatz | 20:bf9ea5125d52 | 288 | |
benkatz | 23:2adf23ee0305 | 289 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 290 | switch(state){ |
benkatz | 37:c0f352d6e8e3 | 291 | case REST_MODE: // Do nothing |
benkatz | 23:2adf23ee0305 | 292 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 293 | enter_menu_state(); |
benkatz | 23:2adf23ee0305 | 294 | } |
benkatz | 55:c4c9fec8539c | 295 | update_observer(&controller, &observer); |
benkatz | 23:2adf23ee0305 | 296 | break; |
benkatz | 22:60276ba87ac6 | 297 | |
benkatz | 23:2adf23ee0305 | 298 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 299 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 300 | calibrate(); |
benkatz | 23:2adf23ee0305 | 301 | } |
benkatz | 23:2adf23ee0305 | 302 | break; |
benkatz | 55:c4c9fec8539c | 303 | case INIT_TEMP_MODE: |
benkatz | 55:c4c9fec8539c | 304 | if(state_change){ |
benkatz | 55:c4c9fec8539c | 305 | enter_torque_mode(); |
benkatz | 55:c4c9fec8539c | 306 | count = 0; |
benkatz | 55:c4c9fec8539c | 307 | observer.resistance = 0.0f; |
benkatz | 55:c4c9fec8539c | 308 | } |
benkatz | 55:c4c9fec8539c | 309 | controller.i_d_ref = -10.0f; |
benkatz | 55:c4c9fec8539c | 310 | controller.i_q_ref = 0.0f; |
benkatz | 55:c4c9fec8539c | 311 | commutate(&controller, &observer, &gpio, controller.theta_elec); |
benkatz | 55:c4c9fec8539c | 312 | |
benkatz | 55:c4c9fec8539c | 313 | if(count > 200) |
benkatz | 55:c4c9fec8539c | 314 | { |
benkatz | 55:c4c9fec8539c | 315 | float r_meas = controller.v_d*(DTC_MAX-DTC_MIN)/(controller.i_d*SQRT3); |
benkatz | 55:c4c9fec8539c | 316 | //testing2[count-100] = controller.i_d; |
benkatz | 55:c4c9fec8539c | 317 | observer.resistance += .001f*r_meas; |
benkatz | 55:c4c9fec8539c | 318 | } |
benkatz | 55:c4c9fec8539c | 319 | if(count > 1200) |
benkatz | 55:c4c9fec8539c | 320 | { |
benkatz | 55:c4c9fec8539c | 321 | count = 0; |
benkatz | 55:c4c9fec8539c | 322 | state = REST_MODE; |
benkatz | 55:c4c9fec8539c | 323 | state_change = 1; |
benkatz | 55:c4c9fec8539c | 324 | gpio.led->write(0); |
benkatz | 55:c4c9fec8539c | 325 | observer.temperature = (double)(T_AMBIENT + ((observer.resistance/R_NOMINAL) - 1.0f)*254.5f); |
benkatz | 55:c4c9fec8539c | 326 | printf("Winding Resistance: %f\n\r", observer.resistance); |
benkatz | 55:c4c9fec8539c | 327 | printf("Winding Temperature: %f\n\r", observer.temperature); |
benkatz | 55:c4c9fec8539c | 328 | |
benkatz | 55:c4c9fec8539c | 329 | if(R_NOMINAL==0) |
benkatz | 55:c4c9fec8539c | 330 | { |
benkatz | 55:c4c9fec8539c | 331 | printf("Saving winding resistance\n\r"); |
benkatz | 55:c4c9fec8539c | 332 | R_NOMINAL = observer.resistance; |
benkatz | 55:c4c9fec8539c | 333 | if (!prefs.ready()) prefs.open(); |
benkatz | 55:c4c9fec8539c | 334 | prefs.flush(); // write offset and lookup table to flash |
benkatz | 55:c4c9fec8539c | 335 | prefs.close(); |
benkatz | 55:c4c9fec8539c | 336 | } |
benkatz | 55:c4c9fec8539c | 337 | //for(int i = 0; i<1000; i++){printf("%f \n\r", testing[i]);} |
benkatz | 55:c4c9fec8539c | 338 | } |
benkatz | 55:c4c9fec8539c | 339 | |
benkatz | 55:c4c9fec8539c | 340 | count++; |
benkatz | 55:c4c9fec8539c | 341 | break; |
benkatz | 26:2b865c00d7e9 | 342 | case MOTOR_MODE: // Run torque control |
benkatz | 25:f5741040c4bb | 343 | if(state_change){ |
benkatz | 25:f5741040c4bb | 344 | enter_torque_mode(); |
benkatz | 28:8c7e29f719c5 | 345 | count = 0; |
adimmit | 77:81c2cbc5d906 | 346 | counter = 0; |
benkatz | 25:f5741040c4bb | 347 | } |
benkatz | 28:8c7e29f719c5 | 348 | else{ |
benkatz | 37:c0f352d6e8e3 | 349 | /* |
benkatz | 37:c0f352d6e8e3 | 350 | 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 | 351 | gpio. |
benkatz | 47:e1196a851f76 | 352 | ->write(0); |
benkatz | 37:c0f352d6e8e3 | 353 | controller.ovp_flag = 1; |
benkatz | 37:c0f352d6e8e3 | 354 | state = REST_MODE; |
benkatz | 37:c0f352d6e8e3 | 355 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 356 | printf("OVP Triggered!\n\r"); |
benkatz | 37:c0f352d6e8e3 | 357 | } |
benkatz | 37:c0f352d6e8e3 | 358 | */ |
benkatz | 37:c0f352d6e8e3 | 359 | |
benkatz | 28:8c7e29f719c5 | 360 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 361 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 362 | controller.i_q_ref = 0; |
benkatz | 37:c0f352d6e8e3 | 363 | controller.kp = 0; |
benkatz | 37:c0f352d6e8e3 | 364 | controller.kd = 0; |
benkatz | 37:c0f352d6e8e3 | 365 | controller.t_ff = 0; |
benkatz | 28:8c7e29f719c5 | 366 | } |
benkatz | 55:c4c9fec8539c | 367 | |
benkatz | 50:ba72df25d10f | 368 | torque_control(&controller); |
benkatz | 55:c4c9fec8539c | 369 | update_observer(&controller, &observer); |
benkatz | 55:c4c9fec8539c | 370 | field_weaken(&controller); |
benkatz | 49:83d83040ea51 | 371 | commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop |
adimmit | 77:81c2cbc5d906 | 372 | |
adimmit | 77:81c2cbc5d906 | 373 | //LOG THE POSITION AND ADD TO THE STACK |
adimmit | 77:81c2cbc5d906 | 374 | for( int i = 0; i<LOG_POINTS-1; i++) { |
adimmit | 77:81c2cbc5d906 | 375 | pos_high_frequency[i] = pos_high_frequency[i+1]; |
adimmit | 77:81c2cbc5d906 | 376 | } |
adimmit | 77:81c2cbc5d906 | 377 | pos_high_frequency[LOG_POINTS-1] = spi.GetMechPosition(); |
adimmit | 77:81c2cbc5d906 | 378 | |
adimmit | 77:81c2cbc5d906 | 379 | if (counter == 1000000) { |
adimmit | 77:81c2cbc5d906 | 380 | pc.printf("Num Failure: %d\n\r", failure_counter); |
adimmit | 77:81c2cbc5d906 | 381 | counter = 0; |
adimmit | 77:81c2cbc5d906 | 382 | } |
adimmit | 77:81c2cbc5d906 | 383 | |
benkatz | 49:83d83040ea51 | 384 | controller.timeout++; |
benkatz | 55:c4c9fec8539c | 385 | |
benkatz | 55:c4c9fec8539c | 386 | if(controller.otw_flag) |
benkatz | 55:c4c9fec8539c | 387 | { |
benkatz | 55:c4c9fec8539c | 388 | state = REST_MODE; |
benkatz | 55:c4c9fec8539c | 389 | state_change = 1; |
benkatz | 55:c4c9fec8539c | 390 | gpio.led->write(0); |
benkatz | 55:c4c9fec8539c | 391 | } |
benkatz | 55:c4c9fec8539c | 392 | |
benkatz | 49:83d83040ea51 | 393 | count++; |
adimmit | 77:81c2cbc5d906 | 394 | counter++; |
benkatz | 55:c4c9fec8539c | 395 | } |
benkatz | 55:c4c9fec8539c | 396 | |
benkatz | 55:c4c9fec8539c | 397 | |
benkatz | 55:c4c9fec8539c | 398 | break; |
benkatz | 37:c0f352d6e8e3 | 399 | |
benkatz | 23:2adf23ee0305 | 400 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 401 | if(state_change){ |
benkatz | 24:58c2d7571207 | 402 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 403 | } |
benkatz | 23:2adf23ee0305 | 404 | break; |
benkatz | 23:2adf23ee0305 | 405 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 406 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 407 | break; |
benkatz | 37:c0f352d6e8e3 | 408 | } |
benkatz | 2:8724412ad628 | 409 | } |
benkatz | 55:c4c9fec8539c | 410 | //gpio.led->write(0); |
benkatz | 23:2adf23ee0305 | 411 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 412 | } |
benkatz | 0:4e1c4df6aabd | 413 | |
benkatz | 25:f5741040c4bb | 414 | |
benkatz | 24:58c2d7571207 | 415 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 416 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 417 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 418 | |
benkatz | 25:f5741040c4bb | 419 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 420 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 421 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 422 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 423 | char c = pc.getc(); |
benkatz | 25:f5741040c4bb | 424 | if(c == 27){ |
benkatz | 25:f5741040c4bb | 425 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 426 | state_change = 1; |
benkatz | 25:f5741040c4bb | 427 | char_count = 0; |
benkatz | 25:f5741040c4bb | 428 | cmd_id = 0; |
benkatz | 55:c4c9fec8539c | 429 | gpio.led->write(0);; |
benkatz | 25:f5741040c4bb | 430 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 431 | } |
benkatz | 24:58c2d7571207 | 432 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 433 | switch (c){ |
benkatz | 23:2adf23ee0305 | 434 | case 'c': |
benkatz | 23:2adf23ee0305 | 435 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 436 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 437 | break; |
benkatz | 26:2b865c00d7e9 | 438 | case 'm': |
benkatz | 26:2b865c00d7e9 | 439 | state = MOTOR_MODE; |
benkatz | 23:2adf23ee0305 | 440 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 441 | break; |
benkatz | 23:2adf23ee0305 | 442 | case 'e': |
benkatz | 23:2adf23ee0305 | 443 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 444 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 445 | break; |
benkatz | 23:2adf23ee0305 | 446 | case 's': |
benkatz | 23:2adf23ee0305 | 447 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 448 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 449 | break; |
benkatz | 37:c0f352d6e8e3 | 450 | case 'z': |
benkatz | 37:c0f352d6e8e3 | 451 | spi.SetMechOffset(0); |
benkatz | 47:e1196a851f76 | 452 | spi.Sample(DT); |
benkatz | 37:c0f352d6e8e3 | 453 | wait_us(20); |
benkatz | 37:c0f352d6e8e3 | 454 | M_OFFSET = spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 455 | if (!prefs.ready()) prefs.open(); |
benkatz | 37:c0f352d6e8e3 | 456 | prefs.flush(); // Write new prefs to flash |
benkatz | 37:c0f352d6e8e3 | 457 | prefs.close(); |
benkatz | 37:c0f352d6e8e3 | 458 | prefs.load(); |
benkatz | 37:c0f352d6e8e3 | 459 | spi.SetMechOffset(M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 460 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 461 | |
benkatz | 37:c0f352d6e8e3 | 462 | break; |
adimmit | 77:81c2cbc5d906 | 463 | case 'p': |
adimmit | 77:81c2cbc5d906 | 464 | //turn off switching |
adimmit | 77:81c2cbc5d906 | 465 | state = REST_MODE; |
adimmit | 77:81c2cbc5d906 | 466 | state_change = 1; |
adimmit | 77:81c2cbc5d906 | 467 | //print the characters to terminal |
adimmit | 77:81c2cbc5d906 | 468 | for (int i=0; i< LOG_POINTS; i++) { |
adimmit | 77:81c2cbc5d906 | 469 | printf("%f\n\r", pos_high_frequency[i]); //return carrige |
adimmit | 77:81c2cbc5d906 | 470 | } |
adimmit | 77:81c2cbc5d906 | 471 | break; |
benkatz | 37:c0f352d6e8e3 | 472 | } |
benkatz | 37:c0f352d6e8e3 | 473 | |
benkatz | 24:58c2d7571207 | 474 | } |
benkatz | 24:58c2d7571207 | 475 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 476 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 477 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 478 | case 'b': |
benkatz | 24:58c2d7571207 | 479 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 480 | break; |
benkatz | 24:58c2d7571207 | 481 | case 'i': |
benkatz | 24:58c2d7571207 | 482 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 483 | break; |
benkatz | 26:2b865c00d7e9 | 484 | case 'm': |
benkatz | 26:2b865c00d7e9 | 485 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 486 | break; |
benkatz | 24:58c2d7571207 | 487 | case 'l': |
elijahsj | 66:3947255b18c5 | 488 | I_MAX = fmaxf(fminf(atof(cmd_val), 65.0f), 0.0f); |
benkatz | 51:6cd89bd6fcaa | 489 | break; |
benkatz | 51:6cd89bd6fcaa | 490 | case 'f': |
benkatz | 51:6cd89bd6fcaa | 491 | I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 492 | break; |
benkatz | 28:8c7e29f719c5 | 493 | case 't': |
benkatz | 28:8c7e29f719c5 | 494 | CAN_TIMEOUT = atoi(cmd_val); |
benkatz | 28:8c7e29f719c5 | 495 | break; |
benkatz | 55:c4c9fec8539c | 496 | case 'h': |
benkatz | 55:c4c9fec8539c | 497 | TEMP_MAX = fmaxf(fminf(atof(cmd_val), 150.0f), 0.0f); |
benkatz | 55:c4c9fec8539c | 498 | break; |
benkatz | 55:c4c9fec8539c | 499 | case 'c': |
benkatz | 55:c4c9fec8539c | 500 | I_MAX_CONT = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f); |
benkatz | 55:c4c9fec8539c | 501 | break; |
adimmit | 71:b314182de37f | 502 | case 's': //CURRENT CONTROL K_SCALE |
adimmit | 71:b314182de37f | 503 | K_SCALE = fmaxf(fminf(atof(cmd_val), 0.1f), 0.0f); |
adimmit | 71:b314182de37f | 504 | break; |
adimmit | 71:b314182de37f | 505 | case 'd': //CURRENT CONTORL KI_D |
adimmit | 71:b314182de37f | 506 | KI_D = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 71:b314182de37f | 507 | break; |
adimmit | 71:b314182de37f | 508 | case 'q': //CURRENT CONTORL KI_Q |
adimmit | 71:b314182de37f | 509 | KI_Q = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 71:b314182de37f | 510 | break; |
adimmit | 73:5676f1232a81 | 511 | case 'r': |
adimmit | 73:5676f1232a81 | 512 | R_PHASE = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 513 | break; |
adimmit | 73:5676f1232a81 | 514 | case 'a': |
adimmit | 73:5676f1232a81 | 515 | L_D = fmaxf(fminf(atof(cmd_val), 0.1f), 0.0f); |
adimmit | 73:5676f1232a81 | 516 | break; |
adimmit | 73:5676f1232a81 | 517 | case 'e': |
adimmit | 73:5676f1232a81 | 518 | L_Q = fmaxf(fminf(atof(cmd_val), 0.1f), 0.0f); |
adimmit | 73:5676f1232a81 | 519 | break; |
adimmit | 75:c7fbacd92b8f | 520 | //case 'n': |
adimmit | 75:c7fbacd92b8f | 521 | //NPP = atoi(cmd_val); |
adimmit | 75:c7fbacd92b8f | 522 | //break; |
adimmit | 73:5676f1232a81 | 523 | case 'p': |
adimmit | 73:5676f1232a81 | 524 | KT = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 525 | break; |
adimmit | 73:5676f1232a81 | 526 | case 'g': |
adimmit | 73:5676f1232a81 | 527 | GR = fmaxf(fminf(atof(cmd_val), 20.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 528 | break; |
adimmit | 73:5676f1232a81 | 529 | case 'k': |
adimmit | 73:5676f1232a81 | 530 | KT_OUT = fmaxf(fminf(atof(cmd_val), 20.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 531 | break; |
adimmit | 73:5676f1232a81 | 532 | case 'w': |
adimmit | 73:5676f1232a81 | 533 | WB = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 534 | break; |
adimmit | 73:5676f1232a81 | 535 | case 'x': |
adimmit | 73:5676f1232a81 | 536 | R_TH = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 537 | break; |
adimmit | 73:5676f1232a81 | 538 | case 'y': |
adimmit | 73:5676f1232a81 | 539 | INV_M_TH = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 540 | break; |
adimmit | 73:5676f1232a81 | 541 | case 'z': |
adimmit | 73:5676f1232a81 | 542 | T_AMBIENT = fmaxf(fminf(atof(cmd_val), 35.0f), 0.0f); |
adimmit | 73:5676f1232a81 | 543 | break; |
benkatz | 24:58c2d7571207 | 544 | default: |
benkatz | 24:58c2d7571207 | 545 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 546 | break; |
benkatz | 24:58c2d7571207 | 547 | } |
benkatz | 24:58c2d7571207 | 548 | |
benkatz | 24:58c2d7571207 | 549 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 550 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 551 | prefs.close(); |
benkatz | 24:58c2d7571207 | 552 | prefs.load(); |
benkatz | 24:58c2d7571207 | 553 | state_change = 1; |
benkatz | 24:58c2d7571207 | 554 | char_count = 0; |
benkatz | 24:58c2d7571207 | 555 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 556 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 557 | } |
benkatz | 24:58c2d7571207 | 558 | else{ |
benkatz | 24:58c2d7571207 | 559 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 560 | else{ |
benkatz | 24:58c2d7571207 | 561 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 562 | |
benkatz | 24:58c2d7571207 | 563 | } |
benkatz | 24:58c2d7571207 | 564 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 565 | char_count++; |
benkatz | 23:2adf23ee0305 | 566 | } |
benkatz | 23:2adf23ee0305 | 567 | } |
benkatz | 24:58c2d7571207 | 568 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 569 | switch (c){ |
benkatz | 24:58c2d7571207 | 570 | case 27: |
benkatz | 24:58c2d7571207 | 571 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 572 | state_change = 1; |
benkatz | 24:58c2d7571207 | 573 | break; |
benkatz | 24:58c2d7571207 | 574 | } |
benkatz | 24:58c2d7571207 | 575 | } |
benkatz | 49:83d83040ea51 | 576 | else if (state == MOTOR_MODE){ |
benkatz | 49:83d83040ea51 | 577 | switch (c){ |
benkatz | 49:83d83040ea51 | 578 | case 'd': |
benkatz | 49:83d83040ea51 | 579 | controller.i_q_ref = 0; |
benkatz | 49:83d83040ea51 | 580 | controller.i_d_ref = 0; |
adimmit | 77:81c2cbc5d906 | 581 | break; |
adimmit | 76:4fd876d4cf2b | 582 | case 'p': |
adimmit | 76:4fd876d4cf2b | 583 | //turn off switching |
adimmit | 76:4fd876d4cf2b | 584 | state = REST_MODE; |
adimmit | 76:4fd876d4cf2b | 585 | state_change = 1; |
adimmit | 76:4fd876d4cf2b | 586 | //print the characters to terminal |
adimmit | 76:4fd876d4cf2b | 587 | for (int i=0; i< LOG_POINTS; i++) { |
adimmit | 76:4fd876d4cf2b | 588 | printf("%f\n\r", pos_high_frequency[i]); //return carrige |
adimmit | 76:4fd876d4cf2b | 589 | } |
adimmit | 77:81c2cbc5d906 | 590 | break; |
benkatz | 49:83d83040ea51 | 591 | } |
benkatz | 49:83d83040ea51 | 592 | } |
benkatz | 24:58c2d7571207 | 593 | |
benkatz | 24:58c2d7571207 | 594 | } |
benkatz | 22:60276ba87ac6 | 595 | } |
benkatz | 0:4e1c4df6aabd | 596 | |
benkatz | 0:4e1c4df6aabd | 597 | int main() { |
benkatz | 20:bf9ea5125d52 | 598 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 599 | controller.mode = 0; |
benkatz | 23:2adf23ee0305 | 600 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 55:c4c9fec8539c | 601 | wait_us(100); |
benkatz | 44:8040fa2fcb0d | 602 | |
benkatz | 44:8040fa2fcb0d | 603 | gpio.enable->write(1); |
benkatz | 44:8040fa2fcb0d | 604 | wait_us(100); |
benkatz | 45:26801179208e | 605 | drv.calibrate(); |
benkatz | 45:26801179208e | 606 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 607 | drv.write_DCR(0x0, DIS_GDF_DIS, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); |
benkatz | 55:c4c9fec8539c | 608 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 609 | drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x1, 0x1, 0x1, SEN_LVL_1_0); // calibrate shunt amplifiers |
benkatz | 44:8040fa2fcb0d | 610 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 611 | zero_current(&controller.adc1_offset, &controller.adc2_offset); |
benkatz | 44:8040fa2fcb0d | 612 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 613 | drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x1, 0x0, 0x0, 0x0, SEN_LVL_1_0); |
benkatz | 55:c4c9fec8539c | 614 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 615 | drv.write_OCPCR(TRETRY_50US, DEADTIME_50NS, OCP_NONE, OCP_DEG_8US, VDS_LVL_1_88); |
benkatz | 45:26801179208e | 616 | |
benkatz | 45:26801179208e | 617 | //drv.enable_gd(); |
benkatz | 47:e1196a851f76 | 618 | drv.disable_gd(); |
benkatz | 55:c4c9fec8539c | 619 | //zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 55:c4c9fec8539c | 620 | //drv.enable_gd(); |
benkatz | 20:bf9ea5125d52 | 621 | |
benkatz | 55:c4c9fec8539c | 622 | wait_us(100); |
benkatz | 55:c4c9fec8539c | 623 | |
benkatz | 23:2adf23ee0305 | 624 | reset_foc(&controller); // Reset current controller |
benkatz | 48:74a40481740c | 625 | reset_observer(&observer); // Reset observer |
benkatz | 26:2b865c00d7e9 | 626 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 627 | |
benkatz | 55:c4c9fec8539c | 628 | wait_us(100); |
benkatz | 37:c0f352d6e8e3 | 629 | NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication |
benkatz | 37:c0f352d6e8e3 | 630 | NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
benkatz | 53:e85efce8c1eb | 631 | // attach 'CAN receive-complete' interrupt handler |
benkatz | 53:e85efce8c1eb | 632 | |
benkatz | 53:e85efce8c1eb | 633 | // If preferences haven't been user configured yet, set defaults |
benkatz | 53:e85efce8c1eb | 634 | prefs.load(); // Read flash |
benkatz | 55:c4c9fec8539c | 635 | can.filter(CAN_ID , 0xFFF, CANStandard, 0); |
benkatz | 28:8c7e29f719c5 | 636 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 637 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 638 | rxMsg.len = 8; |
benkatz | 53:e85efce8c1eb | 639 | can.attach(&onMsgReceived); |
benkatz | 23:2adf23ee0305 | 640 | |
adimmit | 73:5676f1232a81 | 641 | //SETUP ALL FLASH VALUES |
benkatz | 37:c0f352d6e8e3 | 642 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
benkatz | 37:c0f352d6e8e3 | 643 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
benkatz | 48:74a40481740c | 644 | if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} |
benkatz | 51:6cd89bd6fcaa | 645 | if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;} |
benkatz | 55:c4c9fec8539c | 646 | if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=12;} |
benkatz | 48:74a40481740c | 647 | if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} |
benkatz | 48:74a40481740c | 648 | if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} |
benkatz | 55:c4c9fec8539c | 649 | if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 1000;} |
benkatz | 55:c4c9fec8539c | 650 | if(isnan(R_NOMINAL) || R_NOMINAL==-1){R_NOMINAL = 0.0f;} |
benkatz | 55:c4c9fec8539c | 651 | if(isnan(TEMP_MAX) || TEMP_MAX==-1){TEMP_MAX = 125.0f;} |
benkatz | 55:c4c9fec8539c | 652 | if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;} |
adimmit | 73:5676f1232a81 | 653 | //CURRENT CONTROLLER |
adimmit | 71:b314182de37f | 654 | if(isnan(K_SCALE) || K_SCALE==-1){K_SCALE = 0.00013310f;} //CURRENT CONTROLLER K_SCALE |
adimmit | 71:b314182de37f | 655 | if(isnan(KI_D) || KI_D==-1){KI_D = 0.0373f;} //CURRENT CONTROLLER KI_D |
adimmit | 71:b314182de37f | 656 | if(isnan(KI_Q) || KI_Q==-1){KI_Q = 0.0373f;} //CURRENT CONTROLLER KI_Q |
adimmit | 73:5676f1232a81 | 657 | |
adimmit | 73:5676f1232a81 | 658 | //DEFAULT MOTOR PARAMS ARE FOR THE U12 MODULE BELOW |
adimmit | 73:5676f1232a81 | 659 | //MOTOR PARAMS |
adimmit | 73:5676f1232a81 | 660 | if(isnan(R_PHASE) || R_PHASE==-1){R_PHASE = 0.158f;} |
adimmit | 73:5676f1232a81 | 661 | if(isnan(L_D) || L_D==-1){L_D = 0.000084f;} |
adimmit | 73:5676f1232a81 | 662 | if(isnan(L_Q) || L_Q==-1){L_Q = 0.000084f;} |
adimmit | 75:c7fbacd92b8f | 663 | //if(isnan(NPP) || NPP==-1){NPP = 21;} |
adimmit | 73:5676f1232a81 | 664 | //ACTUATOR PARAMS |
adimmit | 73:5676f1232a81 | 665 | if(isnan(KT) || KT==-1){KT = 0.1916f;} |
adimmit | 73:5676f1232a81 | 666 | if(isnan(GR) || GR==-1){GR = 6.0f;} |
adimmit | 73:5676f1232a81 | 667 | if(isnan(KT_OUT) || KT_OUT==-1){KT_OUT = 1.15f;} |
adimmit | 73:5676f1232a81 | 668 | if(isnan(WB) || WB==-1){WB = 0.00608f;} |
adimmit | 73:5676f1232a81 | 669 | //THERMAL OBSERVER |
adimmit | 73:5676f1232a81 | 670 | if(isnan(R_TH) || R_TH==-1){R_TH = 1.25f;} |
adimmit | 73:5676f1232a81 | 671 | if(isnan(INV_M_TH) || INV_M_TH==-1){INV_M_TH = 0.02825f;} |
adimmit | 73:5676f1232a81 | 672 | if(isnan(T_AMBIENT) || T_AMBIENT==-1){T_AMBIENT = 25.0f;} |
adimmit | 73:5676f1232a81 | 673 | |
benkatz | 25:f5741040c4bb | 674 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 37:c0f352d6e8e3 | 675 | spi.SetMechOffset(M_OFFSET); |
benkatz | 56:fe5056ac6740 | 676 | spi.Sample(1.0f); |
benkatz | 56:fe5056ac6740 | 677 | if(spi.GetMechPosition() > PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point |
benkatz | 56:fe5056ac6740 | 678 | else if (spi.GetMechPosition() < -PI){spi.SetMechOffset(M_OFFSET-2.0f*PI);} |
benkatz | 56:fe5056ac6740 | 679 | |
adimmit | 77:81c2cbc5d906 | 680 | //INITIALIZE THE PREVIOUS ANGLES FOR FILTER |
adimmit | 77:81c2cbc5d906 | 681 | prev_mech = spi.GetMechPosition(); |
adimmit | 77:81c2cbc5d906 | 682 | prev_elec = spi.GetElecPosition(); |
adimmit | 77:81c2cbc5d906 | 683 | |
benkatz | 23:2adf23ee0305 | 684 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 685 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 25:f5741040c4bb | 686 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
benkatz | 45:26801179208e | 687 | init_controller_params(&controller); |
benkatz | 55:c4c9fec8539c | 688 | |
benkatz | 45:26801179208e | 689 | |
benkatz | 26:2b865c00d7e9 | 690 | pc.baud(921600); // set serial baud rate |
benkatz | 20:bf9ea5125d52 | 691 | wait(.01); |
adimmit | 73:5676f1232a81 | 692 | pc.printf("\n\r\n\r MIT Biomimetics Motor Driver\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 693 | wait(.01); |
benkatz | 23:2adf23ee0305 | 694 | printf("\n\r Debug Info:\n\r"); |
benkatz | 32:ccac5da77844 | 695 | printf(" Firmware Version: %s\n\r", VERSION_NUM); |
benkatz | 23:2adf23ee0305 | 696 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 697 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 698 | printf(" Output Zero Position: %.4f\n\r", M_OFFSET); |
benkatz | 24:58c2d7571207 | 699 | printf(" CAN ID: %d\n\r", CAN_ID); |
adimmit | 75:c7fbacd92b8f | 700 | printf(" POLE PAIRS: %d\n\r", NPP); |
benkatz | 44:8040fa2fcb0d | 701 | |
benkatz | 44:8040fa2fcb0d | 702 | |
benkatz | 55:c4c9fec8539c | 703 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 44:8040fa2fcb0d | 704 | |
benkatz | 44:8040fa2fcb0d | 705 | |
benkatz | 23:2adf23ee0305 | 706 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 50:ba72df25d10f | 707 | |
benkatz | 20:bf9ea5125d52 | 708 | |
adimmit | 77:81c2cbc5d906 | 709 | int counter = 0; |
benkatz | 0:4e1c4df6aabd | 710 | while(1) { |
benkatz | 55:c4c9fec8539c | 711 | //drv.print_faults(); |
benkatz | 53:e85efce8c1eb | 712 | wait(.1); |
benkatz | 57:0795d2add37e | 713 | |
benkatz | 55:c4c9fec8539c | 714 | if(controller.otw_flag){gpio.led->write(!gpio.led->read());} |
benkatz | 55:c4c9fec8539c | 715 | /* |
benkatz | 47:e1196a851f76 | 716 | if(state == MOTOR_MODE) |
benkatz | 47:e1196a851f76 | 717 | { |
benkatz | 55:c4c9fec8539c | 718 | if(controller.otw_flag){gpio.led->write(!gpio.led->read());} |
benkatz | 55:c4c9fec8539c | 719 | //printf("%f %f\n\r", controller.dtheta_mech, controller.i_d_ref); |
benkatz | 48:74a40481740c | 720 | //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); |
benkatz | 49:83d83040ea51 | 721 | //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); |
benkatz | 55:c4c9fec8539c | 722 | //printf("%.3f %.3f %.3f %.3f\n\r", controller.dtheta_elec, observer.resistance, observer.temperature, observer.temp_measured); |
benkatz | 55:c4c9fec8539c | 723 | //printf("%.3f %.3f\n\r" , observer.temperature, observer.temp_measured); |
benkatz | 47:e1196a851f76 | 724 | } |
benkatz | 55:c4c9fec8539c | 725 | |
benkatz | 50:ba72df25d10f | 726 | */ |
benkatz | 47:e1196a851f76 | 727 | |
benkatz | 0:4e1c4df6aabd | 728 | } |
benkatz | 0:4e1c4df6aabd | 729 | } |