bldc driver firmware based on hobbyking cheetah compact
Dependencies: BLDC_V2 mbed-dev-f303 FastPWM3
main.cpp@47:f4ecf3e0576a, 2020-05-13 (annotated)
- Committer:
- Wooden
- Date:
- Wed May 13 09:53:27 2020 +0000
- Revision:
- 47:f4ecf3e0576a
- Parent:
- 45:aadebe074af6
- Child:
- 48:a74e401a6d84
bldc firmware based on hobbyking cheetah compact
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 | /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling |
benkatz | 22:60276ba87ac6 | 5 | |
benkatz | 23:2adf23ee0305 | 6 | #define REST_MODE 0 |
benkatz | 23:2adf23ee0305 | 7 | #define CALIBRATION_MODE 1 |
benkatz | 26:2b865c00d7e9 | 8 | #define MOTOR_MODE 2 |
benkatz | 23:2adf23ee0305 | 9 | #define SETUP_MODE 4 |
benkatz | 23:2adf23ee0305 | 10 | #define ENCODER_MODE 5 |
Wooden | 47:f4ecf3e0576a | 11 | #define TEST_MODE 6 // this mode is used to test motor, which allow user control motor's torque directly on the GUI software |
benkatz | 22:60276ba87ac6 | 12 | |
benkatz | 44:efcde0af8390 | 13 | #define VERSION_NUM "1.6" |
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" |
Wooden | 47:f4ecf3e0576a | 34 | #include <string> |
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 | 37:c0f352d6e8e3 | 41 | ObserverStruct observer; |
benkatz | 9:d7eb815cb057 | 42 | |
Wooden | 47:f4ecf3e0576a | 43 | // Serial pc(PA_2, PA_3); |
Wooden | 47:f4ecf3e0576a | 44 | Serial pc(PC_6, PC_7); |
Wooden | 47:f4ecf3e0576a | 45 | |
Wooden | 47:f4ecf3e0576a | 46 | DataPackage datapackage; |
benkatz | 17:3c5df2982199 | 47 | |
benkatz | 44:efcde0af8390 | 48 | CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name |
benkatz | 26:2b865c00d7e9 | 49 | CANMessage rxMsg; |
benkatz | 26:2b865c00d7e9 | 50 | CANMessage txMsg; |
benkatz | 23:2adf23ee0305 | 51 | |
Wooden | 47:f4ecf3e0576a | 52 | DigitalOut led_debug(PB_1); |
benkatz | 8:10ae7bc88d6e | 53 | |
benkatz | 26:2b865c00d7e9 | 54 | PositionSensorAM5147 spi(16384, 0.0, NPP); |
benkatz | 20:bf9ea5125d52 | 55 | |
Wooden | 47:f4ecf3e0576a | 56 | //volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 57 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 58 | volatile int state_change; |
benkatz | 20:bf9ea5125d52 | 59 | |
Wooden | 47:f4ecf3e0576a | 60 | volatile float test_tff,test_kp,test_kd,test_pos_des,test_vel_des; |
Wooden | 47:f4ecf3e0576a | 61 | volatile float test_ia,test_ib,test_ic,test_id,test_iq; |
Wooden | 47:f4ecf3e0576a | 62 | volatile float test_mech_angle,test_mech_speed; |
Wooden | 47:f4ecf3e0576a | 63 | |
benkatz | 26:2b865c00d7e9 | 64 | void onMsgReceived() { |
benkatz | 26:2b865c00d7e9 | 65 | //msgAvailable = true; |
Wooden | 47:f4ecf3e0576a | 66 | |
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; |
Wooden | 47:f4ecf3e0576a | 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(); |
benkatz | 28:8c7e29f719c5 | 81 | } |
benkatz | 28:8c7e29f719c5 | 82 | else if(state == MOTOR_MODE){ |
benkatz | 28:8c7e29f719c5 | 83 | unpack_cmd(rxMsg, &controller); |
benkatz | 28:8c7e29f719c5 | 84 | } |
benkatz | 37:c0f352d6e8e3 | 85 | pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); |
benkatz | 37:c0f352d6e8e3 | 86 | can.write(txMsg); |
benkatz | 28:8c7e29f719c5 | 87 | } |
benkatz | 26:2b865c00d7e9 | 88 | |
benkatz | 26:2b865c00d7e9 | 89 | } |
benkatz | 26:2b865c00d7e9 | 90 | |
benkatz | 23:2adf23ee0305 | 91 | void enter_menu_state(void){ |
Wooden | 47:f4ecf3e0576a | 92 | pc.printf("\n\r\n\r\n\r"); |
Wooden | 47:f4ecf3e0576a | 93 | pc.printf(" Commands:\n\r"); |
benkatz | 44:efcde0af8390 | 94 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 95 | pc.printf(" m - Motor Mode\n\r"); |
benkatz | 44:efcde0af8390 | 96 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 97 | pc.printf(" c - Calibrate Encoder\n\r"); |
benkatz | 44:efcde0af8390 | 98 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 99 | pc.printf(" s - Setup\n\r"); |
benkatz | 44:efcde0af8390 | 100 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 101 | pc.printf(" e - Display Encoder\n\r"); |
benkatz | 44:efcde0af8390 | 102 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 103 | pc.printf(" z - Set Zero Position\n\r"); |
benkatz | 44:efcde0af8390 | 104 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 105 | pc.printf(" t - Motor Test Mode\n\r"); |
Wooden | 47:f4ecf3e0576a | 106 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 107 | pc.printf(" esc - Exit to Menu\n\r"); |
benkatz | 44:efcde0af8390 | 108 | wait_us(10); |
benkatz | 23:2adf23ee0305 | 109 | state_change = 0; |
benkatz | 25:f5741040c4bb | 110 | gpio.enable->write(0); |
benkatz | 37:c0f352d6e8e3 | 111 | gpio.led->write(0); |
benkatz | 23:2adf23ee0305 | 112 | } |
benkatz | 24:58c2d7571207 | 113 | |
benkatz | 24:58c2d7571207 | 114 | void enter_setup_state(void){ |
Wooden | 47:f4ecf3e0576a | 115 | pc.printf("\n\r\n\r Configuration Options \n\r\n\n"); |
Wooden | 47:f4ecf3e0576a | 116 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 117 | pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:efcde0af8390 | 118 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 119 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
Wooden | 47:f4ecf3e0576a | 120 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 121 | pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 44:efcde0af8390 | 122 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 123 | pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); |
Wooden | 47:f4ecf3e0576a | 124 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 125 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); |
benkatz | 44:efcde0af8390 | 126 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 127 | pc.printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); |
Wooden | 47:f4ecf3e0576a | 128 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 129 | pc.printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 44:efcde0af8390 | 130 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 131 | state_change = 0; |
Wooden | 47:f4ecf3e0576a | 132 | } |
Wooden | 47:f4ecf3e0576a | 133 | |
Wooden | 47:f4ecf3e0576a | 134 | void enter_test_state(void){ |
Wooden | 47:f4ecf3e0576a | 135 | pc.printf("\n\r\n\r Control Options \n\r\n\n"); |
benkatz | 44:efcde0af8390 | 136 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 137 | pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 44:efcde0af8390 | 138 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 139 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "t", "Torque", "-18", "18", controller.t_ff); |
Wooden | 47:f4ecf3e0576a | 140 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 141 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "k", "Stiffness", "0", "500", controller.kp); |
benkatz | 44:efcde0af8390 | 142 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 143 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "d", "Damping", "0", "5.0", controller.kd); |
Wooden | 47:f4ecf3e0576a | 144 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 145 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "p", "Position", "-12.5", "12.5", controller.p_des); |
Wooden | 47:f4ecf3e0576a | 146 | wait_us(10); |
Wooden | 47:f4ecf3e0576a | 147 | pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Velocity", "-45", "45", controller.v_des); |
benkatz | 44:efcde0af8390 | 148 | wait_us(10); |
benkatz | 24:58c2d7571207 | 149 | state_change = 0; |
benkatz | 24:58c2d7571207 | 150 | } |
benkatz | 22:60276ba87ac6 | 151 | |
benkatz | 23:2adf23ee0305 | 152 | void enter_torque_mode(void){ |
benkatz | 37:c0f352d6e8e3 | 153 | controller.ovp_flag = 0; |
benkatz | 28:8c7e29f719c5 | 154 | gpio.enable->write(1); // Enable gate drive |
benkatz | 28:8c7e29f719c5 | 155 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
benkatz | 28:8c7e29f719c5 | 156 | wait(.001); |
benkatz | 23:2adf23ee0305 | 157 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 158 | controller.i_q_ref = 0; // Current Setpoints |
benkatz | 37:c0f352d6e8e3 | 159 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 160 | state_change = 0; |
Wooden | 47:f4ecf3e0576a | 161 | pc.printf("\n\r Entering Motor Mode \n\r"); |
benkatz | 23:2adf23ee0305 | 162 | } |
benkatz | 22:60276ba87ac6 | 163 | |
benkatz | 23:2adf23ee0305 | 164 | void calibrate(void){ |
benkatz | 25:f5741040c4bb | 165 | gpio.enable->write(1); // Enable gate drive |
benkatz | 37:c0f352d6e8e3 | 166 | gpio.led->write(1); // Turn on status LED |
benkatz | 25:f5741040c4bb | 167 | order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering |
benkatz | 25:f5741040c4bb | 168 | calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure |
benkatz | 37:c0f352d6e8e3 | 169 | gpio.led->write(0);; // Turn off status LED |
benkatz | 23:2adf23ee0305 | 170 | wait(.2); |
benkatz | 25:f5741040c4bb | 171 | gpio.enable->write(0); // Turn off gate drive |
Wooden | 47:f4ecf3e0576a | 172 | pc.printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 23:2adf23ee0305 | 173 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 174 | } |
benkatz | 23:2adf23ee0305 | 175 | |
benkatz | 23:2adf23ee0305 | 176 | void print_encoder(void){ |
Wooden | 47:f4ecf3e0576a | 177 | pc.printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
benkatz | 23:2adf23ee0305 | 178 | wait(.05); |
benkatz | 22:60276ba87ac6 | 179 | } |
benkatz | 20:bf9ea5125d52 | 180 | |
benkatz | 23:2adf23ee0305 | 181 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 182 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
Wooden | 47:f4ecf3e0576a | 183 | int LLLL = 0; |
Wooden | 47:f4ecf3e0576a | 184 | bool led_flag = 0; |
Wooden | 47:f4ecf3e0576a | 185 | bool flag_test_first = true; |
Wooden | 47:f4ecf3e0576a | 186 | |
benkatz | 2:8724412ad628 | 187 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 188 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 23:2adf23ee0305 | 189 | |
benkatz | 23:2adf23ee0305 | 190 | ///Sample current always /// |
benkatz | 25:f5741040c4bb | 191 | ADC1->CR2 |= 0x40000000; // Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 192 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 193 | //for (delay = 0; delay < 55; delay++); |
benkatz | 37:c0f352d6e8e3 | 194 | controller.adc2_raw = ADC2->DR; // Read ADC Data Registers |
benkatz | 23:2adf23ee0305 | 195 | controller.adc1_raw = ADC1->DR; |
benkatz | 37:c0f352d6e8e3 | 196 | controller.adc3_raw = ADC3->DR; |
benkatz | 45:aadebe074af6 | 197 | spi.Sample(DT); // sample position sensor |
benkatz | 37:c0f352d6e8e3 | 198 | controller.theta_elec = spi.GetElecPosition(); |
benkatz | 37:c0f352d6e8e3 | 199 | controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 200 | controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); |
benkatz | 37:c0f352d6e8e3 | 201 | controller.dtheta_elec = spi.GetElecVelocity(); |
benkatz | 37:c0f352d6e8e3 | 202 | controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; |
benkatz | 23:2adf23ee0305 | 203 | /// |
Wooden | 47:f4ecf3e0576a | 204 | LLLL ++; |
Wooden | 47:f4ecf3e0576a | 205 | if(LLLL==10000){ |
Wooden | 47:f4ecf3e0576a | 206 | LLLL=0; |
Wooden | 47:f4ecf3e0576a | 207 | led_flag = 1-led_flag; |
Wooden | 47:f4ecf3e0576a | 208 | led_debug=(led_flag); |
Wooden | 47:f4ecf3e0576a | 209 | // pc.printf("in interrupt\n\r"); |
Wooden | 47:f4ecf3e0576a | 210 | } |
benkatz | 20:bf9ea5125d52 | 211 | |
benkatz | 23:2adf23ee0305 | 212 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 213 | switch(state){ |
benkatz | 37:c0f352d6e8e3 | 214 | case REST_MODE: // Do nothing |
benkatz | 23:2adf23ee0305 | 215 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 216 | enter_menu_state(); |
Wooden | 47:f4ecf3e0576a | 217 | flag_test_first = true; |
benkatz | 23:2adf23ee0305 | 218 | } |
benkatz | 23:2adf23ee0305 | 219 | break; |
benkatz | 22:60276ba87ac6 | 220 | |
benkatz | 23:2adf23ee0305 | 221 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 222 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 223 | calibrate(); |
benkatz | 23:2adf23ee0305 | 224 | } |
benkatz | 23:2adf23ee0305 | 225 | break; |
benkatz | 23:2adf23ee0305 | 226 | |
benkatz | 26:2b865c00d7e9 | 227 | case MOTOR_MODE: // Run torque control |
benkatz | 25:f5741040c4bb | 228 | if(state_change){ |
benkatz | 25:f5741040c4bb | 229 | enter_torque_mode(); |
Wooden | 47:f4ecf3e0576a | 230 | // count = 0; |
benkatz | 25:f5741040c4bb | 231 | } |
benkatz | 28:8c7e29f719c5 | 232 | else{ |
benkatz | 37:c0f352d6e8e3 | 233 | /* |
benkatz | 37:c0f352d6e8e3 | 234 | 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 | 37:c0f352d6e8e3 | 235 | gpio.enable->write(0); |
benkatz | 37:c0f352d6e8e3 | 236 | controller.ovp_flag = 1; |
benkatz | 37:c0f352d6e8e3 | 237 | state = REST_MODE; |
benkatz | 37:c0f352d6e8e3 | 238 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 239 | printf("OVP Triggered!\n\r"); |
benkatz | 37:c0f352d6e8e3 | 240 | } |
benkatz | 37:c0f352d6e8e3 | 241 | */ |
Wooden | 47:f4ecf3e0576a | 242 | // controller.t_ff = 1; |
benkatz | 40:cd7e837b2b93 | 243 | torque_control(&controller); |
benkatz | 28:8c7e29f719c5 | 244 | if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ |
benkatz | 28:8c7e29f719c5 | 245 | controller.i_d_ref = 0; |
benkatz | 28:8c7e29f719c5 | 246 | controller.i_q_ref = 0; |
benkatz | 37:c0f352d6e8e3 | 247 | controller.kp = 0; |
benkatz | 37:c0f352d6e8e3 | 248 | controller.kd = 0; |
benkatz | 37:c0f352d6e8e3 | 249 | controller.t_ff = 0; |
benkatz | 28:8c7e29f719c5 | 250 | } |
benkatz | 37:c0f352d6e8e3 | 251 | commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop |
benkatz | 28:8c7e29f719c5 | 252 | controller.timeout += 1; |
benkatz | 38:67e4e1453a4b | 253 | |
benkatz | 39:3580a907ef93 | 254 | /* |
benkatz | 37:c0f352d6e8e3 | 255 | count++; |
benkatz | 40:cd7e837b2b93 | 256 | if(count == 4000){ |
benkatz | 40:cd7e837b2b93 | 257 | printf("%.4f\n\r", controller.dtheta_mech); |
benkatz | 32:ccac5da77844 | 258 | count = 0; |
benkatz | 23:2adf23ee0305 | 259 | } |
benkatz | 39:3580a907ef93 | 260 | */ |
benkatz | 38:67e4e1453a4b | 261 | |
benkatz | 37:c0f352d6e8e3 | 262 | |
benkatz | 37:c0f352d6e8e3 | 263 | } |
benkatz | 23:2adf23ee0305 | 264 | break; |
benkatz | 23:2adf23ee0305 | 265 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 266 | if(state_change){ |
benkatz | 24:58c2d7571207 | 267 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 268 | } |
benkatz | 23:2adf23ee0305 | 269 | break; |
Wooden | 47:f4ecf3e0576a | 270 | |
Wooden | 47:f4ecf3e0576a | 271 | case TEST_MODE: |
Wooden | 47:f4ecf3e0576a | 272 | if(flag_test_first){ |
Wooden | 47:f4ecf3e0576a | 273 | // enter_test_state(); |
Wooden | 47:f4ecf3e0576a | 274 | // enter_torque_mode(); |
Wooden | 47:f4ecf3e0576a | 275 | controller.ovp_flag = 0; |
Wooden | 47:f4ecf3e0576a | 276 | gpio.enable->write(1); // Enable gate drive |
Wooden | 47:f4ecf3e0576a | 277 | reset_foc(&controller); // Tesets integrators, and other control loop parameters |
Wooden | 47:f4ecf3e0576a | 278 | wait(.001); |
Wooden | 47:f4ecf3e0576a | 279 | controller.i_d_ref = 0; |
Wooden | 47:f4ecf3e0576a | 280 | controller.i_q_ref = 0; // Current Setpoints |
Wooden | 47:f4ecf3e0576a | 281 | gpio.led->write(1); |
Wooden | 47:f4ecf3e0576a | 282 | flag_test_first = false; // Turn on status LED |
Wooden | 47:f4ecf3e0576a | 283 | } |
Wooden | 47:f4ecf3e0576a | 284 | else{ |
Wooden | 47:f4ecf3e0576a | 285 | controller.t_ff = test_tff; |
Wooden | 47:f4ecf3e0576a | 286 | controller.kp = test_kp; |
Wooden | 47:f4ecf3e0576a | 287 | controller.p_des = test_pos_des; |
Wooden | 47:f4ecf3e0576a | 288 | controller.kd = test_kd; |
Wooden | 47:f4ecf3e0576a | 289 | controller.v_des = test_vel_des; |
Wooden | 47:f4ecf3e0576a | 290 | |
Wooden | 47:f4ecf3e0576a | 291 | torque_control(&controller); |
Wooden | 47:f4ecf3e0576a | 292 | commutate(&controller, &observer, &gpio, controller.theta_elec); |
Wooden | 47:f4ecf3e0576a | 293 | test_mech_angle = controller.theta_mech; |
Wooden | 47:f4ecf3e0576a | 294 | test_mech_speed = controller.dtheta_mech; |
Wooden | 47:f4ecf3e0576a | 295 | test_ia = controller.i_a; |
Wooden | 47:f4ecf3e0576a | 296 | test_ib = controller.i_b; |
Wooden | 47:f4ecf3e0576a | 297 | test_ic = controller.i_c; |
Wooden | 47:f4ecf3e0576a | 298 | // test_ia = observer.i_q_est; |
Wooden | 47:f4ecf3e0576a | 299 | // test_ib = controller.v_d; |
Wooden | 47:f4ecf3e0576a | 300 | // test_ic = controller.v_q; |
Wooden | 47:f4ecf3e0576a | 301 | test_id = controller.i_d; |
Wooden | 47:f4ecf3e0576a | 302 | test_iq = controller.i_q; |
Wooden | 47:f4ecf3e0576a | 303 | // test_iq = observer.i_q_est; |
Wooden | 47:f4ecf3e0576a | 304 | // test_id = observer.i_d_est; |
Wooden | 47:f4ecf3e0576a | 305 | // test_iq = controller->i_q_filt; |
Wooden | 47:f4ecf3e0576a | 306 | } |
Wooden | 47:f4ecf3e0576a | 307 | break; |
Wooden | 47:f4ecf3e0576a | 308 | |
benkatz | 23:2adf23ee0305 | 309 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 310 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 311 | break; |
benkatz | 37:c0f352d6e8e3 | 312 | } |
benkatz | 2:8724412ad628 | 313 | } |
benkatz | 23:2adf23ee0305 | 314 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 315 | } |
benkatz | 0:4e1c4df6aabd | 316 | |
benkatz | 25:f5741040c4bb | 317 | |
benkatz | 24:58c2d7571207 | 318 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 319 | char cmd_id = 0; |
benkatz | 25:f5741040c4bb | 320 | char char_count = 0; |
benkatz | 24:58c2d7571207 | 321 | |
benkatz | 25:f5741040c4bb | 322 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 25:f5741040c4bb | 323 | /// Called when data received over serial /// |
benkatz | 23:2adf23ee0305 | 324 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 325 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 326 | char c = pc.getc(); |
benkatz | 25:f5741040c4bb | 327 | if(c == 27){ |
benkatz | 25:f5741040c4bb | 328 | state = REST_MODE; |
benkatz | 25:f5741040c4bb | 329 | state_change = 1; |
benkatz | 25:f5741040c4bb | 330 | char_count = 0; |
benkatz | 25:f5741040c4bb | 331 | cmd_id = 0; |
benkatz | 37:c0f352d6e8e3 | 332 | gpio.led->write(0);; |
benkatz | 25:f5741040c4bb | 333 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 25:f5741040c4bb | 334 | } |
benkatz | 24:58c2d7571207 | 335 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 336 | switch (c){ |
benkatz | 23:2adf23ee0305 | 337 | case 'c': |
benkatz | 23:2adf23ee0305 | 338 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 339 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 340 | break; |
benkatz | 26:2b865c00d7e9 | 341 | case 'm': |
benkatz | 26:2b865c00d7e9 | 342 | state = MOTOR_MODE; |
benkatz | 23:2adf23ee0305 | 343 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 344 | break; |
benkatz | 23:2adf23ee0305 | 345 | case 'e': |
benkatz | 23:2adf23ee0305 | 346 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 347 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 348 | break; |
benkatz | 23:2adf23ee0305 | 349 | case 's': |
benkatz | 23:2adf23ee0305 | 350 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 351 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 352 | break; |
benkatz | 37:c0f352d6e8e3 | 353 | case 'z': |
benkatz | 37:c0f352d6e8e3 | 354 | spi.SetMechOffset(0); |
benkatz | 45:aadebe074af6 | 355 | spi.Sample(DT); |
benkatz | 37:c0f352d6e8e3 | 356 | wait_us(20); |
benkatz | 37:c0f352d6e8e3 | 357 | M_OFFSET = spi.GetMechPosition(); |
benkatz | 37:c0f352d6e8e3 | 358 | if (!prefs.ready()) prefs.open(); |
benkatz | 37:c0f352d6e8e3 | 359 | prefs.flush(); // Write new prefs to flash |
benkatz | 37:c0f352d6e8e3 | 360 | prefs.close(); |
benkatz | 37:c0f352d6e8e3 | 361 | prefs.load(); |
benkatz | 37:c0f352d6e8e3 | 362 | spi.SetMechOffset(M_OFFSET); |
Wooden | 47:f4ecf3e0576a | 363 | pc.printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
benkatz | 37:c0f352d6e8e3 | 364 | |
benkatz | 37:c0f352d6e8e3 | 365 | break; |
Wooden | 47:f4ecf3e0576a | 366 | case 't': |
Wooden | 47:f4ecf3e0576a | 367 | // this state is used to test motor through GUI |
Wooden | 47:f4ecf3e0576a | 368 | state = TEST_MODE; |
Wooden | 47:f4ecf3e0576a | 369 | state_change = 1; |
benkatz | 37:c0f352d6e8e3 | 370 | } |
benkatz | 37:c0f352d6e8e3 | 371 | |
benkatz | 24:58c2d7571207 | 372 | } |
benkatz | 24:58c2d7571207 | 373 | else if(state == SETUP_MODE){ |
benkatz | 25:f5741040c4bb | 374 | if(c == 13){ |
benkatz | 24:58c2d7571207 | 375 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 376 | case 'b': |
benkatz | 24:58c2d7571207 | 377 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 378 | break; |
benkatz | 24:58c2d7571207 | 379 | case 'i': |
benkatz | 24:58c2d7571207 | 380 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 381 | break; |
benkatz | 26:2b865c00d7e9 | 382 | case 'm': |
benkatz | 26:2b865c00d7e9 | 383 | CAN_MASTER = atoi(cmd_val); |
benkatz | 26:2b865c00d7e9 | 384 | break; |
benkatz | 24:58c2d7571207 | 385 | case 'l': |
benkatz | 24:58c2d7571207 | 386 | TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 387 | break; |
benkatz | 28:8c7e29f719c5 | 388 | case 't': |
benkatz | 28:8c7e29f719c5 | 389 | CAN_TIMEOUT = atoi(cmd_val); |
benkatz | 28:8c7e29f719c5 | 390 | break; |
benkatz | 24:58c2d7571207 | 391 | default: |
Wooden | 47:f4ecf3e0576a | 392 | pc.printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 393 | break; |
benkatz | 24:58c2d7571207 | 394 | } |
benkatz | 24:58c2d7571207 | 395 | |
benkatz | 24:58c2d7571207 | 396 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 397 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 398 | prefs.close(); |
benkatz | 24:58c2d7571207 | 399 | prefs.load(); |
benkatz | 24:58c2d7571207 | 400 | state_change = 1; |
benkatz | 24:58c2d7571207 | 401 | char_count = 0; |
benkatz | 24:58c2d7571207 | 402 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 403 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 404 | } |
benkatz | 24:58c2d7571207 | 405 | else{ |
benkatz | 24:58c2d7571207 | 406 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 407 | else{ |
benkatz | 24:58c2d7571207 | 408 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 409 | |
benkatz | 24:58c2d7571207 | 410 | } |
benkatz | 24:58c2d7571207 | 411 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 412 | char_count++; |
benkatz | 23:2adf23ee0305 | 413 | } |
benkatz | 23:2adf23ee0305 | 414 | } |
benkatz | 24:58c2d7571207 | 415 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 416 | switch (c){ |
benkatz | 24:58c2d7571207 | 417 | case 27: |
benkatz | 24:58c2d7571207 | 418 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 419 | state_change = 1; |
benkatz | 24:58c2d7571207 | 420 | break; |
benkatz | 24:58c2d7571207 | 421 | } |
benkatz | 24:58c2d7571207 | 422 | } |
Wooden | 47:f4ecf3e0576a | 423 | else if(state == TEST_MODE){ |
Wooden | 47:f4ecf3e0576a | 424 | if(c == 13){ |
Wooden | 47:f4ecf3e0576a | 425 | switch(cmd_id){ |
Wooden | 47:f4ecf3e0576a | 426 | case 't': |
Wooden | 47:f4ecf3e0576a | 427 | // controller.t_ff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT); |
Wooden | 47:f4ecf3e0576a | 428 | test_tff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT); |
Wooden | 47:f4ecf3e0576a | 429 | break; |
Wooden | 47:f4ecf3e0576a | 430 | case 'k': |
Wooden | 47:f4ecf3e0576a | 431 | // controller.kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN); |
Wooden | 47:f4ecf3e0576a | 432 | test_kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN); |
Wooden | 47:f4ecf3e0576a | 433 | break; |
Wooden | 47:f4ecf3e0576a | 434 | case 'd': |
Wooden | 47:f4ecf3e0576a | 435 | // controller.kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN); |
Wooden | 47:f4ecf3e0576a | 436 | test_kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN); |
Wooden | 47:f4ecf3e0576a | 437 | break; |
Wooden | 47:f4ecf3e0576a | 438 | case 'p': |
Wooden | 47:f4ecf3e0576a | 439 | // controller.p_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN); |
Wooden | 47:f4ecf3e0576a | 440 | test_pos_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN); |
Wooden | 47:f4ecf3e0576a | 441 | break; |
Wooden | 47:f4ecf3e0576a | 442 | case 'v': |
Wooden | 47:f4ecf3e0576a | 443 | // controller.v_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN); |
Wooden | 47:f4ecf3e0576a | 444 | test_vel_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN); |
Wooden | 47:f4ecf3e0576a | 445 | break; |
Wooden | 47:f4ecf3e0576a | 446 | default: |
Wooden | 47:f4ecf3e0576a | 447 | // pc.printf("\n\r '%c' Not a valid command prefix\n\r", cmd_id); |
Wooden | 47:f4ecf3e0576a | 448 | break; |
Wooden | 47:f4ecf3e0576a | 449 | } |
Wooden | 47:f4ecf3e0576a | 450 | state_change = 1; |
Wooden | 47:f4ecf3e0576a | 451 | char_count = 0; |
Wooden | 47:f4ecf3e0576a | 452 | cmd_id = 0; |
Wooden | 47:f4ecf3e0576a | 453 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
Wooden | 47:f4ecf3e0576a | 454 | } |
Wooden | 47:f4ecf3e0576a | 455 | else{ |
Wooden | 47:f4ecf3e0576a | 456 | if(char_count == 0){cmd_id = c;} |
Wooden | 47:f4ecf3e0576a | 457 | else{ |
Wooden | 47:f4ecf3e0576a | 458 | cmd_val[char_count-1] = c; |
Wooden | 47:f4ecf3e0576a | 459 | |
Wooden | 47:f4ecf3e0576a | 460 | } |
Wooden | 47:f4ecf3e0576a | 461 | // pc.putc(c); |
Wooden | 47:f4ecf3e0576a | 462 | char_count++; |
Wooden | 47:f4ecf3e0576a | 463 | } |
Wooden | 47:f4ecf3e0576a | 464 | } |
benkatz | 24:58c2d7571207 | 465 | } |
benkatz | 22:60276ba87ac6 | 466 | } |
benkatz | 0:4e1c4df6aabd | 467 | |
benkatz | 0:4e1c4df6aabd | 468 | int main() { |
benkatz | 45:aadebe074af6 | 469 | |
benkatz | 20:bf9ea5125d52 | 470 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 471 | controller.mode = 0; |
benkatz | 23:2adf23ee0305 | 472 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 20:bf9ea5125d52 | 473 | |
benkatz | 9:d7eb815cb057 | 474 | wait(.1); |
benkatz | 26:2b865c00d7e9 | 475 | gpio.enable->write(1); |
benkatz | 45:aadebe074af6 | 476 | TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles |
benkatz | 45:aadebe074af6 | 477 | TIM1->CCR2 = PWM_ARR*(1.0f); |
benkatz | 45:aadebe074af6 | 478 | TIM1->CCR1 = PWM_ARR*(1.0f); |
benkatz | 23:2adf23ee0305 | 479 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 26:2b865c00d7e9 | 480 | gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 481 | reset_foc(&controller); // Reset current controller |
benkatz | 26:2b865c00d7e9 | 482 | TIM1->CR1 ^= TIM_CR1_UDIS; |
benkatz | 26:2b865c00d7e9 | 483 | //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 484 | |
benkatz | 20:bf9ea5125d52 | 485 | wait(.1); |
benkatz | 37:c0f352d6e8e3 | 486 | NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication |
benkatz | 43:dfb72608639c | 487 | |
benkatz | 37:c0f352d6e8e3 | 488 | NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
benkatz | 26:2b865c00d7e9 | 489 | can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
benkatz | 43:dfb72608639c | 490 | |
benkatz | 28:8c7e29f719c5 | 491 | txMsg.id = CAN_MASTER; |
benkatz | 28:8c7e29f719c5 | 492 | txMsg.len = 6; |
benkatz | 26:2b865c00d7e9 | 493 | rxMsg.len = 8; |
benkatz | 43:dfb72608639c | 494 | can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler |
benkatz | 23:2adf23ee0305 | 495 | |
benkatz | 25:f5741040c4bb | 496 | prefs.load(); // Read flash |
benkatz | 37:c0f352d6e8e3 | 497 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
benkatz | 37:c0f352d6e8e3 | 498 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
benkatz | 25:f5741040c4bb | 499 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
benkatz | 37:c0f352d6e8e3 | 500 | spi.SetMechOffset(M_OFFSET); |
benkatz | 23:2adf23ee0305 | 501 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 502 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 25:f5741040c4bb | 503 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
benkatz | 23:2adf23ee0305 | 504 | |
benkatz | 26:2b865c00d7e9 | 505 | pc.baud(921600); // set serial baud rate |
Wooden | 47:f4ecf3e0576a | 506 | // pc.baud(460800); |
Wooden | 47:f4ecf3e0576a | 507 | |
benkatz | 20:bf9ea5125d52 | 508 | wait(.01); |
Wooden | 47:f4ecf3e0576a | 509 | pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 510 | wait(.01); |
Wooden | 47:f4ecf3e0576a | 511 | pc.printf("\n\r Debug Info:\n\r"); |
Wooden | 47:f4ecf3e0576a | 512 | pc.printf(" Firmware Version: %s\n\r", VERSION_NUM); |
Wooden | 47:f4ecf3e0576a | 513 | pc.printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
Wooden | 47:f4ecf3e0576a | 514 | pc.printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
Wooden | 47:f4ecf3e0576a | 515 | pc.printf(" Output Zero Position: %.4f\n\r", M_OFFSET); |
Wooden | 47:f4ecf3e0576a | 516 | pc.printf(" CAN ID: %d\n\r", CAN_ID); |
benkatz | 23:2adf23ee0305 | 517 | |
benkatz | 23:2adf23ee0305 | 518 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 22:60276ba87ac6 | 519 | |
Wooden | 47:f4ecf3e0576a | 520 | // ============================================================================================================= |
Wooden | 47:f4ecf3e0576a | 521 | // add watch dog here |
Wooden | 47:f4ecf3e0576a | 522 | // Watchdog &watchdog = Watchdog::get_instance(); |
Wooden | 47:f4ecf3e0576a | 523 | // ============================================================================================================= |
Wooden | 47:f4ecf3e0576a | 524 | |
benkatz | 23:2adf23ee0305 | 525 | state_change = 1; |
benkatz | 20:bf9ea5125d52 | 526 | |
Wooden | 47:f4ecf3e0576a | 527 | datapackage.info.init_flag1 = 0xffffffff; |
Wooden | 47:f4ecf3e0576a | 528 | datapackage.info.init_flag2 = 0xfffefdfc; |
Wooden | 47:f4ecf3e0576a | 529 | datapackage.info.check_num = 0; |
benkatz | 0:4e1c4df6aabd | 530 | while(1) { |
Wooden | 47:f4ecf3e0576a | 531 | // pc.printf("hello_wooden"); |
Wooden | 47:f4ecf3e0576a | 532 | // pc.printf("I's still working\n\r"); |
Wooden | 47:f4ecf3e0576a | 533 | // pc.printf("adc1: %d, adc2: %d, adc3: %d",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw); |
Wooden | 47:f4ecf3e0576a | 534 | if(state == TEST_MODE){ |
Wooden | 47:f4ecf3e0576a | 535 | // datapackage.info.I_a = controller.i_a; |
Wooden | 47:f4ecf3e0576a | 536 | // datapackage.info.I_b = controller.i_b; |
Wooden | 47:f4ecf3e0576a | 537 | // datapackage.info.I_c = controller.i_c; |
Wooden | 47:f4ecf3e0576a | 538 | // datapackage.info.I_q = controller.i_q; |
Wooden | 47:f4ecf3e0576a | 539 | // datapackage.info.I_d = controller.i_d; |
Wooden | 47:f4ecf3e0576a | 540 | // datapackage.info.Angle_mech = controller.theta_mech; |
Wooden | 47:f4ecf3e0576a | 541 | // datapackage.info.Angle_elec = controller.theta_elec; |
Wooden | 47:f4ecf3e0576a | 542 | // datapackage.info.Control_tff = controller.t_ff; |
Wooden | 47:f4ecf3e0576a | 543 | // datapackage.info.Control_kp = controller.kp; |
Wooden | 47:f4ecf3e0576a | 544 | // datapackage.info.Control_p = controller.theta_mech; |
Wooden | 47:f4ecf3e0576a | 545 | // datapackage.info.Control_kd = controller.kd; |
Wooden | 47:f4ecf3e0576a | 546 | // datapackage.info.Control_v = controller.dtheta_mech; |
Wooden | 47:f4ecf3e0576a | 547 | |
Wooden | 47:f4ecf3e0576a | 548 | datapackage.info.I_a = test_ia; |
Wooden | 47:f4ecf3e0576a | 549 | datapackage.info.I_b = test_ib; |
Wooden | 47:f4ecf3e0576a | 550 | datapackage.info.I_c = test_ic; |
Wooden | 47:f4ecf3e0576a | 551 | datapackage.info.I_q = test_iq; |
Wooden | 47:f4ecf3e0576a | 552 | datapackage.info.I_d = test_id; |
Wooden | 47:f4ecf3e0576a | 553 | datapackage.info.Angle_mech = test_mech_angle; |
Wooden | 47:f4ecf3e0576a | 554 | datapackage.info.Angle_elec = test_mech_speed; // actually this return the speed |
Wooden | 47:f4ecf3e0576a | 555 | datapackage.info.Control_tff = test_tff; |
Wooden | 47:f4ecf3e0576a | 556 | datapackage.info.Control_kp = test_kp; |
Wooden | 47:f4ecf3e0576a | 557 | datapackage.info.Control_p = test_pos_des; |
Wooden | 47:f4ecf3e0576a | 558 | datapackage.info.Control_kd = test_kd; |
Wooden | 47:f4ecf3e0576a | 559 | datapackage.info.Control_v = test_vel_des; |
Wooden | 47:f4ecf3e0576a | 560 | |
Wooden | 47:f4ecf3e0576a | 561 | // pc.write(datapackage.decode, 60); |
Wooden | 47:f4ecf3e0576a | 562 | // string s(datapackage.decode); |
Wooden | 47:f4ecf3e0576a | 563 | // pc.printf((const char *)datapackage.decode );wait_us(900); |
Wooden | 47:f4ecf3e0576a | 564 | |
Wooden | 47:f4ecf3e0576a | 565 | for(int i=0;i<60;i++){ |
Wooden | 47:f4ecf3e0576a | 566 | pc.putc(datapackage.decode[i]); |
Wooden | 47:f4ecf3e0576a | 567 | // wait_us(30); |
Wooden | 47:f4ecf3e0576a | 568 | wait_us(300); |
Wooden | 47:f4ecf3e0576a | 569 | } |
Wooden | 47:f4ecf3e0576a | 570 | //wait_us(5000); |
Wooden | 47:f4ecf3e0576a | 571 | } |
Wooden | 47:f4ecf3e0576a | 572 | else{ |
Wooden | 47:f4ecf3e0576a | 573 | // can.write(txMsg); |
Wooden | 47:f4ecf3e0576a | 574 | wait(1.0); |
Wooden | 47:f4ecf3e0576a | 575 | ; |
Wooden | 47:f4ecf3e0576a | 576 | } |
benkatz | 11:c83b18d41e54 | 577 | |
benkatz | 0:4e1c4df6aabd | 578 | } |
benkatz | 0:4e1c4df6aabd | 579 | } |