auto-measurements
Dependencies: FastPWM3 mbed-dev
Fork of Hobbyking_Cheetah_Compact by
main.cpp@34:47a55f96fbc4, 2017-10-02 (annotated)
- Committer:
- benkatz
- Date:
- Mon Oct 02 00:55:39 2017 +0000
- Revision:
- 34:47a55f96fbc4
- Parent:
- 32:ccac5da77844
Auto-measurement
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 | 22:60276ba87ac6 | 2 | /// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others |
benkatz | 22:60276ba87ac6 | 3 | /// Hardware documentation can be found at build-its.blogspot.com |
benkatz | 22:60276ba87ac6 | 4 | |
benkatz | 22:60276ba87ac6 | 5 | /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling |
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 | |
benkatz | 34:47a55f96fbc4 | 13 | #define VERSION_NUM "TEST_MODE" |
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 | 34:47a55f96fbc4 | 33 | #include "FastMath.h" |
benkatz | 34:47a55f96fbc4 | 34 | using namespace FastMath; |
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 | 20:bf9ea5125d52 | 40 | COMStruct com; |
benkatz | 22:60276ba87ac6 | 41 | VelocityEstimatorStruct velocity; |
benkatz | 17:3c5df2982199 | 42 | |
benkatz | 9:d7eb815cb057 | 43 | |
benkatz | 26:2b865c00d7e9 | 44 | //using namespace CANnucleo; |
benkatz | 17:3c5df2982199 | 45 | |
benkatz | 26:2b865c00d7e9 | 46 | CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name |
benkatz | 26:2b865c00d7e9 | 47 | CANMessage rxMsg; |
benkatz | 26:2b865c00d7e9 | 48 | CANMessage txMsg; |
benkatz | 23:2adf23ee0305 | 49 | |
benkatz | 20:bf9ea5125d52 | 50 | |
benkatz | 9:d7eb815cb057 | 51 | Serial pc(PA_2, PA_3); |
benkatz | 8:10ae7bc88d6e | 52 | |
benkatz | 26:2b865c00d7e9 | 53 | PositionSensorAM5147 spi(16384, 0.0, NPP); |
benkatz | 26:2b865c00d7e9 | 54 | PositionSensorEncoder encoder(4096, 0, NPP); |
benkatz | 26:2b865c00d7e9 | 55 | |
benkatz | 26:2b865c00d7e9 | 56 | |
benkatz | 26:2b865c00d7e9 | 57 | DigitalOut toggle(PA_0); |
benkatz | 20:bf9ea5125d52 | 58 | |
benkatz | 23:2adf23ee0305 | 59 | volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 60 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 61 | volatile int state_change; |
benkatz | 34:47a55f96fbc4 | 62 | volatile int cal_counter = 0; |
benkatz | 34:47a55f96fbc4 | 63 | volatile float rise = 0; |
benkatz | 34:47a55f96fbc4 | 64 | volatile float avg_current = 0; |
benkatz | 20:bf9ea5125d52 | 65 | |
benkatz | 26:2b865c00d7e9 | 66 | #define P_MIN -12.5f |
benkatz | 26:2b865c00d7e9 | 67 | #define P_MAX 12.5f |
benkatz | 26:2b865c00d7e9 | 68 | #define V_MIN -30.0f |
benkatz | 26:2b865c00d7e9 | 69 | #define V_MAX 30.0f |
benkatz | 26:2b865c00d7e9 | 70 | #define KP_MIN 0.0f |
benkatz | 26:2b865c00d7e9 | 71 | #define KP_MAX 500.0f |
benkatz | 26:2b865c00d7e9 | 72 | #define KD_MIN 0.0f |
benkatz | 28:8c7e29f719c5 | 73 | #define KD_MAX 5.0f |
benkatz | 26:2b865c00d7e9 | 74 | #define T_MIN -18.0f |
benkatz | 26:2b865c00d7e9 | 75 | #define T_MAX 18.0f |
benkatz | 26:2b865c00d7e9 | 76 | |
benkatz | 26:2b865c00d7e9 | 77 | |
benkatz | 26:2b865c00d7e9 | 78 | /// CAN Reply Packet Structure /// |
benkatz | 26:2b865c00d7e9 | 79 | /// 16 bit position, between -4*pi and 4*pi |
benkatz | 26:2b865c00d7e9 | 80 | /// 12 bit velocity, between -30 and + 30 rad/s |
benkatz | 26:2b865c00d7e9 | 81 | /// 12 bit current, between -40 and 40; |
benkatz | 26:2b865c00d7e9 | 82 | /// CAN Packet is 5 8-bit words |
benkatz | 26:2b865c00d7e9 | 83 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 26:2b865c00d7e9 | 84 | /// 0: [position[15-8]] |
benkatz | 26:2b865c00d7e9 | 85 | /// 1: [position[7-0]] |
benkatz | 26:2b865c00d7e9 | 86 | /// 2: [velocity[11-4]] |
benkatz | 26:2b865c00d7e9 | 87 | /// 3: [velocity[3-0], current[11-8]] |
benkatz | 26:2b865c00d7e9 | 88 | /// 4: [current[7-0]] |
benkatz | 26:2b865c00d7e9 | 89 | void pack_reply(CANMessage *msg, float p, float v, float i){ |
benkatz | 26:2b865c00d7e9 | 90 | int p_int = float_to_uint(p, P_MIN, P_MAX, 16); |
benkatz | 26:2b865c00d7e9 | 91 | int v_int = float_to_uint(v, V_MIN, V_MAX, 12); |
benkatz | 26:2b865c00d7e9 | 92 | int i_int = float_to_uint(i, -I_MAX, I_MAX, 12); |
benkatz | 28:8c7e29f719c5 | 93 | msg->data[0] = CAN_ID; |
benkatz | 28:8c7e29f719c5 | 94 | msg->data[1] = p_int>>8; |
benkatz | 28:8c7e29f719c5 | 95 | msg->data[2] = p_int&0xFF; |
benkatz | 28:8c7e29f719c5 | 96 | msg->data[3] = v_int>>4; |
benkatz | 28:8c7e29f719c5 | 97 | msg->data[4] = ((v_int&0xF)<<4) + (i_int>>8); |
benkatz | 28:8c7e29f719c5 | 98 | msg->data[5] = i_int&0xFF; |
benkatz | 26:2b865c00d7e9 | 99 | } |
benkatz | 26:2b865c00d7e9 | 100 | |
benkatz | 26:2b865c00d7e9 | 101 | /// CAN Command Packet Structure /// |
benkatz | 26:2b865c00d7e9 | 102 | /// 16 bit position command, between -4*pi and 4*pi |
benkatz | 26:2b865c00d7e9 | 103 | /// 12 bit velocity command, between -30 and + 30 rad/s |
benkatz | 26:2b865c00d7e9 | 104 | /// 12 bit kp, between 0 and 500 N-m/rad |
benkatz | 26:2b865c00d7e9 | 105 | /// 12 bit kd, between 0 and 100 N-m*s/rad |
benkatz | 26:2b865c00d7e9 | 106 | /// 12 bit feed forward torque, between -18 and 18 N-m |
benkatz | 26:2b865c00d7e9 | 107 | /// CAN Packet is 8 8-bit words |
benkatz | 26:2b865c00d7e9 | 108 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 26:2b865c00d7e9 | 109 | /// 0: [position[15-8]] |
benkatz | 26:2b865c00d7e9 | 110 | /// 1: [position[7-0]] |
benkatz | 26:2b865c00d7e9 | 111 | /// 2: [velocity[11-4]] |
benkatz | 26:2b865c00d7e9 | 112 | /// 3: [velocity[3-0], kp[11-8]] |
benkatz | 26:2b865c00d7e9 | 113 | /// 4: [kp[7-0]] |
benkatz | 26:2b865c00d7e9 | 114 | /// 5: [kd[11-4]] |
benkatz | 26:2b865c00d7e9 | 115 | /// 6: [kd[3-0], torque[11-8]] |
benkatz | 26:2b865c00d7e9 | 116 | /// 7: [torque[7-0]] |
benkatz | 26:2b865c00d7e9 | 117 | void unpack_cmd(CANMessage msg, ControllerStruct * controller){ |
benkatz | 28:8c7e29f719c5 | 118 | int p_int = (msg.data[0]<<8)|msg.data[1]; |
benkatz | 28:8c7e29f719c5 | 119 | int v_int = (msg.data[2]<<4)|(msg.data[3]>>4); |
benkatz | 28:8c7e29f719c5 | 120 | int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; |
benkatz | 28:8c7e29f719c5 | 121 | int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); |
benkatz | 28:8c7e29f719c5 | 122 | int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7]; |
benkatz | 28:8c7e29f719c5 | 123 | |
benkatz | 28:8c7e29f719c5 | 124 | controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16); |
benkatz | 28:8c7e29f719c5 | 125 | controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12); |
benkatz | 28:8c7e29f719c5 | 126 | controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12); |
benkatz | 28:8c7e29f719c5 | 127 | controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12); |
benkatz | 28:8c7e29f719c5 | 128 | controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12); |
benkatz | 26:2b865c00d7e9 | 129 | |
benkatz | 26:2b865c00d7e9 | 130 | |
benkatz | 28:8c7e29f719c5 | 131 | //printf("Received "); |
benkatz | 28:8c7e29f719c5 | 132 | //printf("%.3f %.3f %.3f %.3f %.3f %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref); |
benkatz | 28:8c7e29f719c5 | 133 | //printf("\n\r"); |
benkatz | 28:8c7e29f719c5 | 134 | |
benkatz | 26:2b865c00d7e9 | 135 | |
benkatz | 26:2b865c00d7e9 | 136 | } |
benkatz | 26:2b865c00d7e9 | 137 | |
benkatz | 26:2b865c00d7e9 | 138 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 139 | //msgAvailable = true; |
benkatz | 26:2b865c00d7e9 | 140 | //printf("%.3f %.3f %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q); |
benkatz | 26:2b865c00d7e9 | 141 | can.read(rxMsg); |
benkatz | 28:8c7e29f719c5 | 142 | |
benkatz | 28:8c7e29f719c5 | 143 | if((rxMsg.id == CAN_ID)){ |
benkatz | 28:8c7e29f719c5 | 144 | controller.timeout = 0; |
benkatz | 28:8c7e29f719c5 | 145 | 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 | 146 | state = MOTOR_MODE; |
benkatz | 28:8c7e29f719c5 | 147 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 148 | } |
benkatz | 28:8c7e29f719c5 | 149 | 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 | 150 | state = REST_MODE; |
benkatz | 28:8c7e29f719c5 | 151 | state_change = 1; |
benkatz | 28:8c7e29f719c5 | 152 | GPIOC->ODR &= !(1 << 5); |
benkatz | 28:8c7e29f719c5 | 153 | } |
benkatz | 28:8c7e29f719c5 | 154 | 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 | 155 | spi.ZeroPosition(); |
benkatz | 28:8c7e29f719c5 | 156 | } |
benkatz | 28:8c7e29f719c5 | 157 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 158 | unpack_cmd(rxMsg, &controller); |
benkatz | 28:8c7e29f719c5 | 159 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q); |
benkatz | 28:8c7e29f719c5 | 160 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 161 | } |
benkatz | 28:8c7e29f719c5 | 162 | } |
benkatz | 26:2b865c00d7e9 | 163 | |
benkatz | 26:2b865c00d7e9 | 164 | } |
benkatz | 26:2b865c00d7e9 | 165 | |
benkatz | 23:2adf23ee0305 | 166 | void enter_menu_state(void){ |
benkatz | 23:2adf23ee0305 | 167 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 168 | printf(" Commands:\n\r"); |
benkatz | 26:2b865c00d7e9 | 169 | printf(" m - Motor Mode\n\r"); |
benkatz | 23:2adf23ee0305 | 170 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 23:2adf23ee0305 | 171 | printf(" s - Setup\n\r"); |
benkatz | 23:2adf23ee0305 | 172 | printf(" e - Display Encoder\n\r"); |
benkatz | 23:2adf23ee0305 | 173 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 23:2adf23ee0305 | 174 | state_change = 0; |
benkatz | 25:f5741040c4bb | 175 | gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 176 | } |
benkatz | 24:58c2d7571207 | 177 | |
benkatz | 24:58c2d7571207 | 178 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 179 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 28:8c7e29f719c5 | 180 | printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 28:8c7e29f719c5 | 181 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 28:8c7e29f719c5 | 182 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 28:8c7e29f719c5 | 183 | printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
benkatz | 28:8c7e29f719c5 | 184 | printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); |
benkatz | 28:8c7e29f719c5 | 185 | printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
benkatz | 24:58c2d7571207 | 186 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 24:58c2d7571207 | 187 | state_change = 0; |
benkatz | 24:58c2d7571207 | 188 | } |
benkatz | 22:60276ba87ac6 | 189 | |
benkatz | 23:2adf23ee0305 | 190 | void enter_torque_mode(void){ |
benkatz | 28:8c7e29f719c5 | 191 | gpio.enable->write(1); // Enable gate drive |
benkatz | 28:8c7e29f719c5 | 192 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 193 | wait(.001); |
benkatz | 23:2adf23ee0305 | 194 | controller.i_d_ref = 0; |
benkatz | 34:47a55f96fbc4 | 195 | controller.i_q_ref = 2; // Current Setpoints |
benkatz | 26:2b865c00d7e9 | 196 | GPIOC->ODR |= (1 << 5); // Turn on status LED |
benkatz | 25:f5741040c4bb | 197 | state_change = 0; |
benkatz | 28:8c7e29f719c5 | 198 | printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 199 | } |
benkatz | 22:60276ba87ac6 | 200 | |
benkatz | 23:2adf23ee0305 | 201 | void calibrate(void){ |
benkatz | 25:f5741040c4bb | 202 | gpio.enable->write(1); // Enable gate drive |
benkatz | 26:2b865c00d7e9 | 203 | GPIOC->ODR |= (1 << 5); // Turn on status LED |
benkatz | 25:f5741040c4bb | 204 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 205 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
benkatz | 26:2b865c00d7e9 | 206 | GPIOC->ODR &= !(1 << 5); // Turn off status LED |
benkatz | 23:2adf23ee0305 | 207 | wait(.2); |
benkatz | 25:f5741040c4bb | 208 | gpio.enable->write(0); // Turn off gate drive |
benkatz | 23:2adf23ee0305 | 209 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 23:2adf23ee0305 | 210 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 211 | |
benkatz | 23:2adf23ee0305 | 212 | } |
benkatz | 23:2adf23ee0305 | 213 | |
benkatz | 23:2adf23ee0305 | 214 | void print_encoder(void){ |
benkatz | 23:2adf23ee0305 | 215 | spi.Sample(); |
benkatz | 23:2adf23ee0305 | 216 | wait(.001); |
benkatz | 23:2adf23ee0305 | 217 | printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
benkatz | 23:2adf23ee0305 | 218 | wait(.05); |
benkatz | 22:60276ba87ac6 | 219 | } |
benkatz | 20:bf9ea5125d52 | 220 | |
benkatz | 23:2adf23ee0305 | 221 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 222 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
benkatz | 2:8724412ad628 | 223 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 224 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 20:bf9ea5125d52 | 225 | //toggle = 1; |
benkatz | 23:2adf23ee0305 | 226 | |
benkatz | 23:2adf23ee0305 | 227 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 228 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 229 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 230 | //for (delay = 0; delay < 55; delay++); |
benkatz | 25:f5741040c4bb | 231 | controller.adc2_raw = ADC2->DR; // Read ADC1 and ADC2 Data Registers |
benkatz | 23:2adf23ee0305 | 232 | controller.adc1_raw = ADC1->DR; |
benkatz | 23:2adf23ee0305 | 233 | /// |
benkatz | 20:bf9ea5125d52 | 234 | |
benkatz | 23:2adf23ee0305 | 235 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 236 | //printf("%d\n\r", state); |
benkatz | 23:2adf23ee0305 | 237 | switch(state){ |
benkatz | 23:2adf23ee0305 | 238 | case REST_MODE: // Do nothing until |
benkatz | 23:2adf23ee0305 | 239 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 240 | enter_menu_state(); |
benkatz | 23:2adf23ee0305 | 241 | } |
benkatz | 23:2adf23ee0305 | 242 | break; |
benkatz | 22:60276ba87ac6 | 243 | |
benkatz | 23:2adf23ee0305 | 244 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 245 | if(state_change){ |
benkatz | 34:47a55f96fbc4 | 246 | gpio.enable->write(1); |
benkatz | 34:47a55f96fbc4 | 247 | |
benkatz | 34:47a55f96fbc4 | 248 | //calibrate(); |
benkatz | 34:47a55f96fbc4 | 249 | state_change = 0; |
benkatz | 34:47a55f96fbc4 | 250 | GPIOC->ODR |= (1 << 5); |
benkatz | 34:47a55f96fbc4 | 251 | TIM1->CCR3 = (PWM_ARR)*(0.5f); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 252 | TIM1->CCR2 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 253 | TIM1->CCR1 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 254 | wait_us(200); |
benkatz | 34:47a55f96fbc4 | 255 | cal_counter = 1; |
benkatz | 34:47a55f96fbc4 | 256 | rise = 0; |
benkatz | 34:47a55f96fbc4 | 257 | avg_current = 0; |
benkatz | 34:47a55f96fbc4 | 258 | } |
benkatz | 34:47a55f96fbc4 | 259 | if(cal_counter>0){ |
benkatz | 34:47a55f96fbc4 | 260 | if(cal_counter<1000){ |
benkatz | 34:47a55f96fbc4 | 261 | TIM1->CCR3 = (PWM_ARR)*(0.5f); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 262 | TIM1->CCR2 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 263 | TIM1->CCR1 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 264 | } |
benkatz | 34:47a55f96fbc4 | 265 | if(cal_counter>=1000){ |
benkatz | 34:47a55f96fbc4 | 266 | float s = FastSin(0); |
benkatz | 34:47a55f96fbc4 | 267 | float c = FastCos(0); |
benkatz | 34:47a55f96fbc4 | 268 | float dtc_u, dtc_v, dtc_w; |
benkatz | 34:47a55f96fbc4 | 269 | float i_a, i_b, i_c, i_d, i_q = 0.0f; |
benkatz | 34:47a55f96fbc4 | 270 | controller.v_d = 3.0f; |
benkatz | 34:47a55f96fbc4 | 271 | controller.v_q = 0.0f; |
benkatz | 34:47a55f96fbc4 | 272 | float v_u = c*controller.v_d - s*controller.v_q; // Faster Inverse DQ0 transform |
benkatz | 34:47a55f96fbc4 | 273 | float v_v = (0.86602540378f*s-.5f*c)*controller.v_d - (-0.86602540378f*c-.5f*s)*controller.v_q; |
benkatz | 34:47a55f96fbc4 | 274 | float v_w = (-0.86602540378f*s-.5f*c)*controller.v_d - (0.86602540378f*c-.5f*s)*controller.v_q; |
benkatz | 34:47a55f96fbc4 | 275 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation |
benkatz | 34:47a55f96fbc4 | 276 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 277 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 278 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 34:47a55f96fbc4 | 279 | i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset); // Calculate phase currents from ADC readings |
benkatz | 34:47a55f96fbc4 | 280 | i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset); |
benkatz | 34:47a55f96fbc4 | 281 | i_a = -i_b - i_c; |
benkatz | 34:47a55f96fbc4 | 282 | i_d = 0.6666667f*(c*i_a + (0.86602540378f*s-.5f*c)*i_b + (-0.86602540378f*s-.5f*c)*i_c); ///Faster DQ0 Transform |
benkatz | 34:47a55f96fbc4 | 283 | i_q = 0.6666667f*(-s*i_a - (-0.86602540378f*c-.5f*s)*i_b - (0.86602540378f*c-.5f*s)*i_c); |
benkatz | 34:47a55f96fbc4 | 284 | float current = sqrt(i_d*i_d + i_q*i_q); |
benkatz | 34:47a55f96fbc4 | 285 | avg_current += .000025f*current; |
benkatz | 34:47a55f96fbc4 | 286 | if(cal_counter<1020){rise += .05f*current;} |
benkatz | 34:47a55f96fbc4 | 287 | } |
benkatz | 34:47a55f96fbc4 | 288 | cal_counter++; |
benkatz | 34:47a55f96fbc4 | 289 | if(cal_counter>41000){ |
benkatz | 34:47a55f96fbc4 | 290 | cal_counter = 0; |
benkatz | 34:47a55f96fbc4 | 291 | GPIOC->ODR &= !(1 << 5); |
benkatz | 34:47a55f96fbc4 | 292 | wait_us(200); |
benkatz | 34:47a55f96fbc4 | 293 | gpio.enable->write(0); |
benkatz | 34:47a55f96fbc4 | 294 | float L = controller.v_d*.0005f/(2.0f*rise); |
benkatz | 34:47a55f96fbc4 | 295 | float R = controller.v_d/avg_current; |
benkatz | 34:47a55f96fbc4 | 296 | printf("%f \n\r", rise); |
benkatz | 34:47a55f96fbc4 | 297 | calibrate(); |
benkatz | 34:47a55f96fbc4 | 298 | printf("Inductance: %f\n\r", L); |
benkatz | 34:47a55f96fbc4 | 299 | printf("Resistance: %f\n\r", R); |
benkatz | 34:47a55f96fbc4 | 300 | |
benkatz | 34:47a55f96fbc4 | 301 | } |
benkatz | 34:47a55f96fbc4 | 302 | |
benkatz | 23:2adf23ee0305 | 303 | } |
benkatz | 23:2adf23ee0305 | 304 | break; |
benkatz | 23:2adf23ee0305 | 305 | |
benkatz | 26:2b865c00d7e9 | 306 | case MOTOR_MODE: // Run torque control |
benkatz | 25:f5741040c4bb | 307 | if(state_change){ |
benkatz | 25:f5741040c4bb | 308 | enter_torque_mode(); |
benkatz | 28:8c7e29f719c5 | 309 | count = 0; |
benkatz | 25:f5741040c4bb | 310 | } |
benkatz | 28:8c7e29f719c5 | 311 | else{ |
benkatz | 23:2adf23ee0305 | 312 | count++; |
benkatz | 28:8c7e29f719c5 | 313 | //toggle.write(1); |
benkatz | 26:2b865c00d7e9 | 314 | controller.theta_elec = spi.GetElecPosition(); |
benkatz | 28:8c7e29f719c5 | 315 | controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); |
benkatz | 28:8c7e29f719c5 | 316 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); |
benkatz | 26:2b865c00d7e9 | 317 | //TIM1->CCR3 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 318 | //TIM1->CCR1 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 319 | //TIM1->CCR2 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 320 | |
benkatz | 26:2b865c00d7e9 | 321 | //controller.i_q_ref = controller.t_ff/KT_OUT; |
benkatz | 34:47a55f96fbc4 | 322 | if(count >40){ |
benkatz | 34:47a55f96fbc4 | 323 | controller.i_q_ref = 20; |
benkatz | 34:47a55f96fbc4 | 324 | //wait(.001); |
benkatz | 34:47a55f96fbc4 | 325 | //printf(" Started commutating\n\r"); |
benkatz | 34:47a55f96fbc4 | 326 | } |
benkatz | 34:47a55f96fbc4 | 327 | if(count>80){ |
benkatz | 34:47a55f96fbc4 | 328 | controller.i_q_ref = -20; |
benkatz | 34:47a55f96fbc4 | 329 | count = 0; |
benkatz | 34:47a55f96fbc4 | 330 | } |
benkatz | 34:47a55f96fbc4 | 331 | //torque_control(&controller); |
benkatz | 28:8c7e29f719c5 | 332 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 333 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 334 | controller.i_q_ref = 0; |
benkatz | 28:8c7e29f719c5 | 335 | } |
benkatz | 28:8c7e29f719c5 | 336 | //controller.i_q_ref = .5; |
benkatz | 23:2adf23ee0305 | 337 | commutate(&controller, &gpio, controller.theta_elec); // Run current loop |
benkatz | 23:2adf23ee0305 | 338 | spi.Sample(); // Sample position sensor |
benkatz | 28:8c7e29f719c5 | 339 | //toggle.write(0); |
benkatz | 28:8c7e29f719c5 | 340 | controller.timeout += 1; |
benkatz | 26:2b865c00d7e9 | 341 | |
benkatz | 34:47a55f96fbc4 | 342 | } |
benkatz | 28:8c7e29f719c5 | 343 | |
benkatz | 23:2adf23ee0305 | 344 | break; |
benkatz | 34:47a55f96fbc4 | 345 | |
benkatz | 23:2adf23ee0305 | 346 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 347 | if(state_change){ |
benkatz | 24:58c2d7571207 | 348 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 349 | } |
benkatz | 23:2adf23ee0305 | 350 | break; |
benkatz | 23:2adf23ee0305 | 351 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 352 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 353 | break; |
benkatz | 23:2adf23ee0305 | 354 | } |
benkatz | 34:47a55f96fbc4 | 355 | |
benkatz | 26:2b865c00d7e9 | 356 | |
benkatz | 2:8724412ad628 | 357 | } |
benkatz | 23:2adf23ee0305 | 358 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 359 | } |
benkatz | 0:4e1c4df6aabd | 360 | |
benkatz | 25:f5741040c4bb | 361 | |
benkatz | 24:58c2d7571207 | 362 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 363 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 364 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 365 | |
benkatz | 25:f5741040c4bb | 366 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 367 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 368 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 369 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 370 | char c = pc.getc(); |
benkatz | 25:f5741040c4bb | 371 | if(c == 27){ |
benkatz | 25:f5741040c4bb | 372 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 373 | state_change = 1; |
benkatz | 25:f5741040c4bb | 374 | char_count = 0; |
benkatz | 25:f5741040c4bb | 375 | cmd_id = 0; |
benkatz | 26:2b865c00d7e9 | 376 | GPIOC->ODR &= !(1 << 5); |
benkatz | 25:f5741040c4bb | 377 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 378 | } |
benkatz | 24:58c2d7571207 | 379 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 380 | switch (c){ |
benkatz | 23:2adf23ee0305 | 381 | case 'c': |
benkatz | 23:2adf23ee0305 | 382 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 383 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 384 | break; |
benkatz | 26:2b865c00d7e9 | 385 | case 'm': |
benkatz | 26:2b865c00d7e9 | 386 | state = MOTOR_MODE; |
benkatz | 23:2adf23ee0305 | 387 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 388 | break; |
benkatz | 23:2adf23ee0305 | 389 | case 'e': |
benkatz | 23:2adf23ee0305 | 390 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 391 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 392 | break; |
benkatz | 23:2adf23ee0305 | 393 | case 's': |
benkatz | 23:2adf23ee0305 | 394 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 395 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 396 | break; |
benkatz | 24:58c2d7571207 | 397 | } |
benkatz | 24:58c2d7571207 | 398 | } |
benkatz | 24:58c2d7571207 | 399 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 400 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 401 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 402 | case 'b': |
benkatz | 24:58c2d7571207 | 403 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 404 | break; |
benkatz | 24:58c2d7571207 | 405 | case 'i': |
benkatz | 24:58c2d7571207 | 406 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 407 | break; |
benkatz | 26:2b865c00d7e9 | 408 | case 'm': |
benkatz | 26:2b865c00d7e9 | 409 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 410 | break; |
benkatz | 24:58c2d7571207 | 411 | case 'l': |
benkatz | 24:58c2d7571207 | 412 | TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 413 | break; |
benkatz | 28:8c7e29f719c5 | 414 | case 't': |
benkatz | 28:8c7e29f719c5 | 415 | CAN_TIMEOUT = atoi(cmd_val); |
benkatz | 28:8c7e29f719c5 | 416 | break; |
benkatz | 24:58c2d7571207 | 417 | default: |
benkatz | 24:58c2d7571207 | 418 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 419 | break; |
benkatz | 24:58c2d7571207 | 420 | } |
benkatz | 24:58c2d7571207 | 421 | |
benkatz | 24:58c2d7571207 | 422 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 423 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 424 | prefs.close(); |
benkatz | 24:58c2d7571207 | 425 | prefs.load(); |
benkatz | 24:58c2d7571207 | 426 | state_change = 1; |
benkatz | 24:58c2d7571207 | 427 | char_count = 0; |
benkatz | 24:58c2d7571207 | 428 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 429 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 430 | } |
benkatz | 24:58c2d7571207 | 431 | else{ |
benkatz | 24:58c2d7571207 | 432 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 433 | else{ |
benkatz | 24:58c2d7571207 | 434 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 435 | |
benkatz | 24:58c2d7571207 | 436 | } |
benkatz | 24:58c2d7571207 | 437 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 438 | char_count++; |
benkatz | 23:2adf23ee0305 | 439 | } |
benkatz | 23:2adf23ee0305 | 440 | } |
benkatz | 24:58c2d7571207 | 441 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 442 | switch (c){ |
benkatz | 24:58c2d7571207 | 443 | case 27: |
benkatz | 24:58c2d7571207 | 444 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 445 | state_change = 1; |
benkatz | 24:58c2d7571207 | 446 | break; |
benkatz | 24:58c2d7571207 | 447 | } |
benkatz | 24:58c2d7571207 | 448 | } |
benkatz | 24:58c2d7571207 | 449 | |
benkatz | 24:58c2d7571207 | 450 | } |
benkatz | 22:60276ba87ac6 | 451 | } |
benkatz | 0:4e1c4df6aabd | 452 | |
benkatz | 0:4e1c4df6aabd | 453 | int main() { |
benkatz | 23:2adf23ee0305 | 454 | |
benkatz | 20:bf9ea5125d52 | 455 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 456 | controller.mode = 0; |
benkatz | 23:2adf23ee0305 | 457 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 20:bf9ea5125d52 | 458 | |
benkatz | 9:d7eb815cb057 | 459 | wait(.1); |
benkatz | 26:2b865c00d7e9 | 460 | gpio.enable->write(1); |
benkatz | 26:2b865c00d7e9 | 461 | TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles |
benkatz | 26:2b865c00d7e9 | 462 | TIM1->CCR2 = 0x708*(1.0f); |
benkatz | 26:2b865c00d7e9 | 463 | TIM1->CCR1 = 0x708*(1.0f); |
benkatz | 23:2adf23ee0305 | 464 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 26:2b865c00d7e9 | 465 | gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 466 | reset_foc(&controller); // Reset current controller |
benkatz | 26:2b865c00d7e9 | 467 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 26:2b865c00d7e9 | 468 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 469 | |
benkatz | 20:bf9ea5125d52 | 470 | wait(.1); |
benkatz | 23:2adf23ee0305 | 471 | NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority |
benkatz | 22:60276ba87ac6 | 472 | |
benkatz | 26:2b865c00d7e9 | 473 | |
benkatz | 23:2adf23ee0305 | 474 | can.frequency(1000000); // set bit rate to 1Mbps |
benkatz | 26:2b865c00d7e9 | 475 | can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
benkatz | 26:2b865c00d7e9 | 476 | //can.filter(CAN_ID, 0xF, CANStandard, 0); |
benkatz | 26:2b865c00d7e9 | 477 | can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler |
benkatz | 28:8c7e29f719c5 | 478 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 479 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 480 | rxMsg.len = 8; |
benkatz | 23:2adf23ee0305 | 481 | |
benkatz | 25:f5741040c4bb | 482 | prefs.load(); // Read flash |
benkatz | 25:f5741040c4bb | 483 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 23:2adf23ee0305 | 484 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 485 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 25:f5741040c4bb | 486 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
benkatz | 23:2adf23ee0305 | 487 | |
benkatz | 26:2b865c00d7e9 | 488 | pc.baud(921600); // set serial baud rate |
benkatz | 20:bf9ea5125d52 | 489 | wait(.01); |
benkatz | 23:2adf23ee0305 | 490 | pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 491 | wait(.01); |
benkatz | 23:2adf23ee0305 | 492 | printf("\n\r Debug Info:\n\r"); |
benkatz | 32:ccac5da77844 | 493 | printf(" Firmware Version: %s\n\r", VERSION_NUM); |
benkatz | 23:2adf23ee0305 | 494 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 495 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 24:58c2d7571207 | 496 | printf(" CAN ID: %d\n\r", CAN_ID); |
benkatz | 23:2adf23ee0305 | 497 | |
benkatz | 23:2adf23ee0305 | 498 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 22:60276ba87ac6 | 499 | |
benkatz | 23:2adf23ee0305 | 500 | state_change = 1; |
benkatz | 20:bf9ea5125d52 | 501 | |
benkatz | 22:60276ba87ac6 | 502 | |
benkatz | 0:4e1c4df6aabd | 503 | while(1) { |
benkatz | 11:c83b18d41e54 | 504 | |
benkatz | 0:4e1c4df6aabd | 505 | } |
benkatz | 0:4e1c4df6aabd | 506 | } |