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