modified 0511
Dependencies: mbed-dev FastPWM3
main.cpp@24:58c2d7571207, 2017-04-07 (annotated)
- Committer:
- benkatz
- Date:
- Fri Apr 07 16:23:39 2017 +0000
- Revision:
- 24:58c2d7571207
- Parent:
- 23:2adf23ee0305
- Child:
- 25:f5741040c4bb
Can flush preferences multiple times now
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 | 23:2adf23ee0305 | 9 | #define TORQUE_MODE 2 |
benkatz | 23:2adf23ee0305 | 10 | #define PD_MODE 3 |
benkatz | 23:2adf23ee0305 | 11 | #define SETUP_MODE 4 |
benkatz | 23:2adf23ee0305 | 12 | #define ENCODER_MODE 5 |
benkatz | 22:60276ba87ac6 | 13 | |
benkatz | 22:60276ba87ac6 | 14 | |
benkatz | 17:3c5df2982199 | 15 | const unsigned int BOARDNUM = 0x2; |
benkatz | 17:3c5df2982199 | 16 | //const unsigned int a_id = |
benkatz | 17:3c5df2982199 | 17 | const unsigned int TX_ID = 0x0100; |
benkatz | 18:f1d56f4acb39 | 18 | const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7; |
benkatz | 18:f1d56f4acb39 | 19 | |
benkatz | 23:2adf23ee0305 | 20 | float __float_reg[64]; |
benkatz | 23:2adf23ee0305 | 21 | int __int_reg[256]; |
benkatz | 17:3c5df2982199 | 22 | |
benkatz | 17:3c5df2982199 | 23 | #include "CANnucleo.h" |
benkatz | 0:4e1c4df6aabd | 24 | #include "mbed.h" |
benkatz | 0:4e1c4df6aabd | 25 | #include "PositionSensor.h" |
benkatz | 20:bf9ea5125d52 | 26 | #include "structs.h" |
benkatz | 20:bf9ea5125d52 | 27 | #include "foc.h" |
benkatz | 22:60276ba87ac6 | 28 | #include "calibration.h" |
benkatz | 20:bf9ea5125d52 | 29 | #include "hw_setup.h" |
benkatz | 23:2adf23ee0305 | 30 | #include "math_ops.h" |
benkatz | 20:bf9ea5125d52 | 31 | #include "current_controller_config.h" |
benkatz | 20:bf9ea5125d52 | 32 | #include "hw_config.h" |
benkatz | 20:bf9ea5125d52 | 33 | #include "motor_config.h" |
benkatz | 23:2adf23ee0305 | 34 | #include "stm32f4xx_flash.h" |
benkatz | 23:2adf23ee0305 | 35 | #include "FlashWriter.h" |
benkatz | 23:2adf23ee0305 | 36 | #include "user_config.h" |
benkatz | 23:2adf23ee0305 | 37 | #include "PreferenceWriter.h" |
benkatz | 23:2adf23ee0305 | 38 | |
benkatz | 23:2adf23ee0305 | 39 | PreferenceWriter prefs(6); |
benkatz | 9:d7eb815cb057 | 40 | |
benkatz | 20:bf9ea5125d52 | 41 | GPIOStruct gpio; |
benkatz | 20:bf9ea5125d52 | 42 | ControllerStruct controller; |
benkatz | 20:bf9ea5125d52 | 43 | COMStruct com; |
benkatz | 22:60276ba87ac6 | 44 | VelocityEstimatorStruct velocity; |
benkatz | 17:3c5df2982199 | 45 | |
benkatz | 9:d7eb815cb057 | 46 | |
benkatz | 23:2adf23ee0305 | 47 | CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name |
benkatz | 17:3c5df2982199 | 48 | CANnucleo::CANMessage rxMsg; |
benkatz | 17:3c5df2982199 | 49 | CANnucleo::CANMessage txMsg; |
benkatz | 17:3c5df2982199 | 50 | int ledState; |
benkatz | 17:3c5df2982199 | 51 | int counter = 0; |
benkatz | 18:f1d56f4acb39 | 52 | int canCmd = 1000; |
benkatz | 17:3c5df2982199 | 53 | volatile bool msgAvailable = false; |
benkatz | 17:3c5df2982199 | 54 | |
benkatz | 20:bf9ea5125d52 | 55 | DigitalOut toggle(PA_0); |
benkatz | 20:bf9ea5125d52 | 56 | Ticker loop; |
benkatz | 17:3c5df2982199 | 57 | /** |
benkatz | 17:3c5df2982199 | 58 | * @brief 'CAN receive-complete' interrup handler. |
benkatz | 17:3c5df2982199 | 59 | * @note Called on arrival of new CAN message. |
benkatz | 17:3c5df2982199 | 60 | * Keep it as short as possible. |
benkatz | 17:3c5df2982199 | 61 | * @param |
benkatz | 17:3c5df2982199 | 62 | * @retval |
benkatz | 17:3c5df2982199 | 63 | */ |
benkatz | 17:3c5df2982199 | 64 | void onMsgReceived() { |
benkatz | 17:3c5df2982199 | 65 | msgAvailable = true; |
benkatz | 17:3c5df2982199 | 66 | //printf("ping\n\r"); |
benkatz | 17:3c5df2982199 | 67 | } |
benkatz | 17:3c5df2982199 | 68 | |
benkatz | 17:3c5df2982199 | 69 | void sendCMD(int TX_addr, int val){ |
benkatz | 17:3c5df2982199 | 70 | txMsg.clear(); //clear Tx message storage |
benkatz | 17:3c5df2982199 | 71 | txMsg.id = TX_addr; |
benkatz | 17:3c5df2982199 | 72 | txMsg << val; |
benkatz | 17:3c5df2982199 | 73 | can.write(txMsg); |
benkatz | 17:3c5df2982199 | 74 | //wait(.1); |
benkatz | 17:3c5df2982199 | 75 | |
benkatz | 17:3c5df2982199 | 76 | } |
benkatz | 17:3c5df2982199 | 77 | |
benkatz | 18:f1d56f4acb39 | 78 | void readCAN(void){ |
benkatz | 18:f1d56f4acb39 | 79 | if(msgAvailable) { |
benkatz | 23:2adf23ee0305 | 80 | msgAvailable = false; // reset flag for next use |
benkatz | 23:2adf23ee0305 | 81 | can.read(rxMsg); // read message into Rx message storage |
benkatz | 18:f1d56f4acb39 | 82 | // Filtering performed by software: |
benkatz | 23:2adf23ee0305 | 83 | if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware |
benkatz | 23:2adf23ee0305 | 84 | rxMsg >> canCmd; // extract first data item |
benkatz | 23:2adf23ee0305 | 85 | } |
benkatz | 18:f1d56f4acb39 | 86 | } |
benkatz | 23:2adf23ee0305 | 87 | } |
benkatz | 17:3c5df2982199 | 88 | |
benkatz | 20:bf9ea5125d52 | 89 | void cancontroller(void){ |
benkatz | 18:f1d56f4acb39 | 90 | //printf("%d\n\r", canCmd); |
benkatz | 18:f1d56f4acb39 | 91 | readCAN(); |
benkatz | 18:f1d56f4acb39 | 92 | //sendCMD(TX_ID, canCmd); |
benkatz | 23:2adf23ee0305 | 93 | |
benkatz | 17:3c5df2982199 | 94 | } |
benkatz | 17:3c5df2982199 | 95 | |
benkatz | 20:bf9ea5125d52 | 96 | |
benkatz | 9:d7eb815cb057 | 97 | Serial pc(PA_2, PA_3); |
benkatz | 8:10ae7bc88d6e | 98 | |
benkatz | 23:2adf23ee0305 | 99 | PositionSensorAM5147 spi(16384, 0.0, NPP); |
benkatz | 22:60276ba87ac6 | 100 | PositionSensorEncoder encoder(4096, 0, 21); |
benkatz | 20:bf9ea5125d52 | 101 | |
benkatz | 23:2adf23ee0305 | 102 | volatile int count = 0; |
benkatz | 23:2adf23ee0305 | 103 | volatile int state = REST_MODE; |
benkatz | 23:2adf23ee0305 | 104 | volatile int state_change; |
benkatz | 20:bf9ea5125d52 | 105 | |
benkatz | 23:2adf23ee0305 | 106 | void enter_menu_state(void){ |
benkatz | 23:2adf23ee0305 | 107 | printf("\n\r\n\r\n\r"); |
benkatz | 23:2adf23ee0305 | 108 | printf(" Commands:\n\r"); |
benkatz | 23:2adf23ee0305 | 109 | printf(" t - Torque Mode\n\r"); |
benkatz | 23:2adf23ee0305 | 110 | printf(" p - PD Mode\n\r"); |
benkatz | 23:2adf23ee0305 | 111 | printf(" c - Calibrate Encoder\n\r"); |
benkatz | 23:2adf23ee0305 | 112 | printf(" s - Setup\n\r"); |
benkatz | 23:2adf23ee0305 | 113 | printf(" e - Display Encoder\n\r"); |
benkatz | 23:2adf23ee0305 | 114 | printf(" esc - Exit to Menu\n\r"); |
benkatz | 23:2adf23ee0305 | 115 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 116 | } |
benkatz | 24:58c2d7571207 | 117 | |
benkatz | 24:58c2d7571207 | 118 | void enter_setup_state(void){ |
benkatz | 24:58c2d7571207 | 119 | printf("\n\r\n\r Configuration Options \n\r\n\n"); |
benkatz | 24:58c2d7571207 | 120 | printf(" %-7s %-25s %-5s %-5s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); |
benkatz | 24:58c2d7571207 | 121 | printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); |
benkatz | 24:58c2d7571207 | 122 | printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); |
benkatz | 24:58c2d7571207 | 123 | printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); |
benkatz | 24:58c2d7571207 | 124 | printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); |
benkatz | 24:58c2d7571207 | 125 | state_change = 0; |
benkatz | 24:58c2d7571207 | 126 | } |
benkatz | 22:60276ba87ac6 | 127 | |
benkatz | 23:2adf23ee0305 | 128 | void enter_torque_mode(void){ |
benkatz | 23:2adf23ee0305 | 129 | controller.i_d_ref = 0; |
benkatz | 23:2adf23ee0305 | 130 | controller.i_q_ref = 0; |
benkatz | 23:2adf23ee0305 | 131 | reset_foc(&controller); //resets integrators, and other control loop parameters |
benkatz | 23:2adf23ee0305 | 132 | gpio.enable->write(1); |
benkatz | 23:2adf23ee0305 | 133 | GPIOC->ODR ^= (1 << 5); //turn on status LED |
benkatz | 23:2adf23ee0305 | 134 | } |
benkatz | 22:60276ba87ac6 | 135 | |
benkatz | 23:2adf23ee0305 | 136 | void calibrate(void){ |
benkatz | 23:2adf23ee0305 | 137 | gpio.enable->write(1); |
benkatz | 23:2adf23ee0305 | 138 | GPIOC->ODR ^= (1 << 5); |
benkatz | 23:2adf23ee0305 | 139 | order_phases(&spi, &gpio, &controller, &prefs); |
benkatz | 23:2adf23ee0305 | 140 | calibrate(&spi, &gpio, &controller, &prefs); |
benkatz | 23:2adf23ee0305 | 141 | GPIOC->ODR ^= (1 << 5); |
benkatz | 23:2adf23ee0305 | 142 | wait(.2); |
benkatz | 23:2adf23ee0305 | 143 | gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 144 | printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); |
benkatz | 23:2adf23ee0305 | 145 | state_change = 0; |
benkatz | 23:2adf23ee0305 | 146 | |
benkatz | 23:2adf23ee0305 | 147 | } |
benkatz | 23:2adf23ee0305 | 148 | |
benkatz | 23:2adf23ee0305 | 149 | void print_encoder(void){ |
benkatz | 23:2adf23ee0305 | 150 | spi.Sample(); |
benkatz | 23:2adf23ee0305 | 151 | wait(.001); |
benkatz | 23:2adf23ee0305 | 152 | printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); |
benkatz | 23:2adf23ee0305 | 153 | wait(.05); |
benkatz | 22:60276ba87ac6 | 154 | } |
benkatz | 20:bf9ea5125d52 | 155 | |
benkatz | 23:2adf23ee0305 | 156 | /// Current Sampling Interrupt /// |
benkatz | 23:2adf23ee0305 | 157 | /// This runs at 40 kHz, regardless of of the mode the controller is in /// |
benkatz | 2:8724412ad628 | 158 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
benkatz | 2:8724412ad628 | 159 | if (TIM1->SR & TIM_SR_UIF ) { |
benkatz | 20:bf9ea5125d52 | 160 | //toggle = 1; |
benkatz | 23:2adf23ee0305 | 161 | |
benkatz | 23:2adf23ee0305 | 162 | ///Sample current always /// |
benkatz | 23:2adf23ee0305 | 163 | ADC1->CR2 |= 0x40000000; //Begin sample and conversion |
benkatz | 22:60276ba87ac6 | 164 | //volatile int delay; |
benkatz | 20:bf9ea5125d52 | 165 | //for (delay = 0; delay < 55; delay++); |
benkatz | 23:2adf23ee0305 | 166 | controller.adc2_raw = ADC2->DR; |
benkatz | 23:2adf23ee0305 | 167 | controller.adc1_raw = ADC1->DR; |
benkatz | 23:2adf23ee0305 | 168 | /// |
benkatz | 20:bf9ea5125d52 | 169 | |
benkatz | 23:2adf23ee0305 | 170 | /// Check state machine state, and run the appropriate function /// |
benkatz | 23:2adf23ee0305 | 171 | //printf("%d\n\r", state); |
benkatz | 23:2adf23ee0305 | 172 | switch(state){ |
benkatz | 23:2adf23ee0305 | 173 | case REST_MODE: // Do nothing until |
benkatz | 23:2adf23ee0305 | 174 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 175 | enter_menu_state(); |
benkatz | 23:2adf23ee0305 | 176 | } |
benkatz | 23:2adf23ee0305 | 177 | break; |
benkatz | 22:60276ba87ac6 | 178 | |
benkatz | 23:2adf23ee0305 | 179 | case CALIBRATION_MODE: // Run encoder calibration procedure |
benkatz | 23:2adf23ee0305 | 180 | if(state_change){ |
benkatz | 23:2adf23ee0305 | 181 | calibrate(); |
benkatz | 23:2adf23ee0305 | 182 | } |
benkatz | 23:2adf23ee0305 | 183 | break; |
benkatz | 23:2adf23ee0305 | 184 | |
benkatz | 23:2adf23ee0305 | 185 | case TORQUE_MODE: // Run torque control |
benkatz | 23:2adf23ee0305 | 186 | count++; |
benkatz | 23:2adf23ee0305 | 187 | controller.theta_elec = spi.GetElecPosition(); |
benkatz | 23:2adf23ee0305 | 188 | commutate(&controller, &gpio, controller.theta_elec); // Run current loop |
benkatz | 23:2adf23ee0305 | 189 | spi.Sample(); // Sample position sensor |
benkatz | 23:2adf23ee0305 | 190 | if(count > 100){ |
benkatz | 23:2adf23ee0305 | 191 | count = 0; |
benkatz | 23:2adf23ee0305 | 192 | readCAN(); |
benkatz | 23:2adf23ee0305 | 193 | controller.i_q_ref = ((float)(canCmd-1000))/100; |
benkatz | 23:2adf23ee0305 | 194 | //pc.printf("%f\n\r ", controller.theta_elec); |
benkatz | 23:2adf23ee0305 | 195 | } |
benkatz | 23:2adf23ee0305 | 196 | break; |
benkatz | 23:2adf23ee0305 | 197 | |
benkatz | 23:2adf23ee0305 | 198 | case PD_MODE: |
benkatz | 23:2adf23ee0305 | 199 | break; |
benkatz | 23:2adf23ee0305 | 200 | case SETUP_MODE: |
benkatz | 23:2adf23ee0305 | 201 | if(state_change){ |
benkatz | 24:58c2d7571207 | 202 | enter_setup_state(); |
benkatz | 23:2adf23ee0305 | 203 | } |
benkatz | 23:2adf23ee0305 | 204 | break; |
benkatz | 23:2adf23ee0305 | 205 | case ENCODER_MODE: |
benkatz | 23:2adf23ee0305 | 206 | print_encoder(); |
benkatz | 23:2adf23ee0305 | 207 | break; |
benkatz | 23:2adf23ee0305 | 208 | } |
benkatz | 23:2adf23ee0305 | 209 | |
benkatz | 23:2adf23ee0305 | 210 | |
benkatz | 22:60276ba87ac6 | 211 | |
benkatz | 2:8724412ad628 | 212 | } |
benkatz | 23:2adf23ee0305 | 213 | TIM1->SR = 0x0; // reset the status register |
benkatz | 2:8724412ad628 | 214 | } |
benkatz | 0:4e1c4df6aabd | 215 | |
benkatz | 23:2adf23ee0305 | 216 | /// Manage state machine with commands from serial terminal or configurator gui /// |
benkatz | 24:58c2d7571207 | 217 | char cmd_val[8] = {0}; |
benkatz | 24:58c2d7571207 | 218 | char cmd_id = 0; |
benkatz | 24:58c2d7571207 | 219 | |
benkatz | 24:58c2d7571207 | 220 | char char_count = 0; |
benkatz | 23:2adf23ee0305 | 221 | void serial_interrupt(void){ |
benkatz | 23:2adf23ee0305 | 222 | while(pc.readable()){ |
benkatz | 23:2adf23ee0305 | 223 | char c = pc.getc(); |
benkatz | 24:58c2d7571207 | 224 | if(state == REST_MODE){ |
benkatz | 23:2adf23ee0305 | 225 | switch (c){ |
benkatz | 24:58c2d7571207 | 226 | case 27: |
benkatz | 24:58c2d7571207 | 227 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 228 | state_change = 1; |
benkatz | 24:58c2d7571207 | 229 | break; |
benkatz | 23:2adf23ee0305 | 230 | case 'c': |
benkatz | 23:2adf23ee0305 | 231 | state = CALIBRATION_MODE; |
benkatz | 23:2adf23ee0305 | 232 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 233 | break; |
benkatz | 23:2adf23ee0305 | 234 | case 't': |
benkatz | 23:2adf23ee0305 | 235 | state = TORQUE_MODE; |
benkatz | 23:2adf23ee0305 | 236 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 237 | break; |
benkatz | 23:2adf23ee0305 | 238 | case 'e': |
benkatz | 23:2adf23ee0305 | 239 | state = ENCODER_MODE; |
benkatz | 23:2adf23ee0305 | 240 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 241 | break; |
benkatz | 23:2adf23ee0305 | 242 | case 's': |
benkatz | 23:2adf23ee0305 | 243 | state = SETUP_MODE; |
benkatz | 23:2adf23ee0305 | 244 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 245 | break; |
benkatz | 24:58c2d7571207 | 246 | } |
benkatz | 24:58c2d7571207 | 247 | } |
benkatz | 24:58c2d7571207 | 248 | else if(state == SETUP_MODE){ |
benkatz | 24:58c2d7571207 | 249 | if(c == 27){ |
benkatz | 24:58c2d7571207 | 250 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 251 | state_change = 1; |
benkatz | 24:58c2d7571207 | 252 | char_count = 0; |
benkatz | 24:58c2d7571207 | 253 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 254 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 255 | } |
benkatz | 24:58c2d7571207 | 256 | else if(c == 13){ |
benkatz | 24:58c2d7571207 | 257 | switch (cmd_id){ |
benkatz | 24:58c2d7571207 | 258 | case 'b': |
benkatz | 24:58c2d7571207 | 259 | I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); |
benkatz | 24:58c2d7571207 | 260 | break; |
benkatz | 24:58c2d7571207 | 261 | case 'i': |
benkatz | 24:58c2d7571207 | 262 | CAN_ID = atoi(cmd_val); |
benkatz | 24:58c2d7571207 | 263 | break; |
benkatz | 24:58c2d7571207 | 264 | case 'l': |
benkatz | 24:58c2d7571207 | 265 | TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); |
benkatz | 24:58c2d7571207 | 266 | break; |
benkatz | 24:58c2d7571207 | 267 | default: |
benkatz | 24:58c2d7571207 | 268 | printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); |
benkatz | 24:58c2d7571207 | 269 | break; |
benkatz | 24:58c2d7571207 | 270 | } |
benkatz | 24:58c2d7571207 | 271 | |
benkatz | 24:58c2d7571207 | 272 | if (!prefs.ready()) prefs.open(); |
benkatz | 24:58c2d7571207 | 273 | prefs.flush(); // Write new prefs to flash |
benkatz | 24:58c2d7571207 | 274 | prefs.close(); |
benkatz | 24:58c2d7571207 | 275 | prefs.load(); |
benkatz | 24:58c2d7571207 | 276 | state_change = 1; |
benkatz | 24:58c2d7571207 | 277 | char_count = 0; |
benkatz | 24:58c2d7571207 | 278 | cmd_id = 0; |
benkatz | 24:58c2d7571207 | 279 | for(int i = 0; i<8; i++){cmd_val[i] = 0;} |
benkatz | 24:58c2d7571207 | 280 | } |
benkatz | 24:58c2d7571207 | 281 | else{ |
benkatz | 24:58c2d7571207 | 282 | if(char_count == 0){cmd_id = c;} |
benkatz | 24:58c2d7571207 | 283 | else{ |
benkatz | 24:58c2d7571207 | 284 | cmd_val[char_count-1] = c; |
benkatz | 24:58c2d7571207 | 285 | |
benkatz | 24:58c2d7571207 | 286 | } |
benkatz | 24:58c2d7571207 | 287 | pc.putc(c); |
benkatz | 24:58c2d7571207 | 288 | char_count++; |
benkatz | 23:2adf23ee0305 | 289 | } |
benkatz | 23:2adf23ee0305 | 290 | } |
benkatz | 24:58c2d7571207 | 291 | else if (state == ENCODER_MODE){ |
benkatz | 24:58c2d7571207 | 292 | switch (c){ |
benkatz | 24:58c2d7571207 | 293 | case 27: |
benkatz | 24:58c2d7571207 | 294 | state = REST_MODE; |
benkatz | 24:58c2d7571207 | 295 | state_change = 1; |
benkatz | 24:58c2d7571207 | 296 | break; |
benkatz | 24:58c2d7571207 | 297 | } |
benkatz | 24:58c2d7571207 | 298 | } |
benkatz | 24:58c2d7571207 | 299 | |
benkatz | 24:58c2d7571207 | 300 | } |
benkatz | 22:60276ba87ac6 | 301 | } |
benkatz | 0:4e1c4df6aabd | 302 | |
benkatz | 0:4e1c4df6aabd | 303 | int main() { |
benkatz | 23:2adf23ee0305 | 304 | |
benkatz | 23:2adf23ee0305 | 305 | |
benkatz | 20:bf9ea5125d52 | 306 | |
benkatz | 20:bf9ea5125d52 | 307 | controller.v_bus = V_BUS; |
benkatz | 22:60276ba87ac6 | 308 | controller.mode = 0; |
benkatz | 22:60276ba87ac6 | 309 | //spi.ZeroPosition(); |
benkatz | 23:2adf23ee0305 | 310 | Init_All_HW(&gpio); // Setup PWM, ADC, GPIO |
benkatz | 20:bf9ea5125d52 | 311 | |
benkatz | 9:d7eb815cb057 | 312 | wait(.1); |
benkatz | 20:bf9ea5125d52 | 313 | //TIM1->CR1 |= TIM_CR1_UDIS; |
benkatz | 23:2adf23ee0305 | 314 | gpio.enable->write(1); // Enable gate drive |
benkatz | 23:2adf23ee0305 | 315 | gpio.pwm_u->write(1.0f); // Write duty cycles |
benkatz | 20:bf9ea5125d52 | 316 | gpio.pwm_v->write(1.0f); |
benkatz | 20:bf9ea5125d52 | 317 | gpio.pwm_w->write(1.0f); |
benkatz | 23:2adf23ee0305 | 318 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset |
benkatz | 22:60276ba87ac6 | 319 | //gpio.enable->write(0); |
benkatz | 23:2adf23ee0305 | 320 | reset_foc(&controller); // Reset current controller |
benkatz | 22:60276ba87ac6 | 321 | |
benkatz | 23:2adf23ee0305 | 322 | TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt |
benkatz | 20:bf9ea5125d52 | 323 | |
benkatz | 20:bf9ea5125d52 | 324 | wait(.1); |
benkatz | 23:2adf23ee0305 | 325 | NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority |
benkatz | 22:60276ba87ac6 | 326 | |
benkatz | 23:2adf23ee0305 | 327 | can.frequency(1000000); // set bit rate to 1Mbps |
benkatz | 23:2adf23ee0305 | 328 | can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler |
benkatz | 18:f1d56f4acb39 | 329 | can.filter(0x020 << 25, 0xF0000004, CANAny, 0); |
benkatz | 18:f1d56f4acb39 | 330 | |
benkatz | 23:2adf23ee0305 | 331 | |
benkatz | 23:2adf23ee0305 | 332 | prefs.load(); |
benkatz | 23:2adf23ee0305 | 333 | spi.SetElecOffset(E_OFFSET); |
benkatz | 23:2adf23ee0305 | 334 | int lut[128] = {0}; |
benkatz | 23:2adf23ee0305 | 335 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
benkatz | 23:2adf23ee0305 | 336 | spi.WriteLUT(lut); |
benkatz | 23:2adf23ee0305 | 337 | |
benkatz | 23:2adf23ee0305 | 338 | pc.baud(115200); // set serial baud rate |
benkatz | 20:bf9ea5125d52 | 339 | wait(.01); |
benkatz | 23:2adf23ee0305 | 340 | pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); |
benkatz | 20:bf9ea5125d52 | 341 | wait(.01); |
benkatz | 23:2adf23ee0305 | 342 | printf("\n\r Debug Info:\n\r"); |
benkatz | 23:2adf23ee0305 | 343 | printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); |
benkatz | 23:2adf23ee0305 | 344 | printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); |
benkatz | 24:58c2d7571207 | 345 | printf(" CAN ID: %d\n\r", CAN_ID); |
benkatz | 23:2adf23ee0305 | 346 | |
benkatz | 23:2adf23ee0305 | 347 | pc.attach(&serial_interrupt); // attach serial interrupt |
benkatz | 22:60276ba87ac6 | 348 | |
benkatz | 23:2adf23ee0305 | 349 | state_change = 1; |
benkatz | 23:2adf23ee0305 | 350 | //enter_menu_state(); //Print available commands, wait for command |
benkatz | 23:2adf23ee0305 | 351 | //enter_calibration_mode(); |
benkatz | 23:2adf23ee0305 | 352 | //wait_us(100); |
benkatz | 23:2adf23ee0305 | 353 | |
benkatz | 23:2adf23ee0305 | 354 | //enter_torque_mode(); |
benkatz | 20:bf9ea5125d52 | 355 | |
benkatz | 22:60276ba87ac6 | 356 | |
benkatz | 0:4e1c4df6aabd | 357 | while(1) { |
benkatz | 11:c83b18d41e54 | 358 | |
benkatz | 0:4e1c4df6aabd | 359 | } |
benkatz | 0:4e1c4df6aabd | 360 | } |