Ikhee Jo / T-Motor_AK80_BaseCode

BaseCode for T-Motor_AK80

Dependencies:   mbed-dev-f303 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?

UserRevisionLine numberNew 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 }