Motor control for robots. More compact, less object-oriented revision.

Dependencies:   FastPWM3 mbed-dev-f303

Fork of Hobbyking_Cheetah_V1 by Ben Katz

Committer:
benkatz
Date:
Sun Apr 09 03:05:52 2017 +0000
Revision:
25:f5741040c4bb
Parent:
24:58c2d7571207
Child:
26:2b865c00d7e9
Set current bandwidth from serial interface

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 25:f5741040c4bb 116 gpio.enable->write(0);
benkatz 23:2adf23ee0305 117 }
benkatz 24:58c2d7571207 118
benkatz 24:58c2d7571207 119 void enter_setup_state(void){
benkatz 24:58c2d7571207 120 printf("\n\r\n\r Configuration Options \n\r\n\n");
benkatz 24:58c2d7571207 121 printf(" %-7s %-25s %-5s %-5s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
benkatz 24:58c2d7571207 122 printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
benkatz 24:58c2d7571207 123 printf(" %-7s %-25s %-5s %-5s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
benkatz 24:58c2d7571207 124 printf(" %-7s %-25s %-5s %-5s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
benkatz 24:58c2d7571207 125 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
benkatz 24:58c2d7571207 126 state_change = 0;
benkatz 24:58c2d7571207 127 }
benkatz 22:60276ba87ac6 128
benkatz 23:2adf23ee0305 129 void enter_torque_mode(void){
benkatz 23:2adf23ee0305 130 controller.i_d_ref = 0;
benkatz 25:f5741040c4bb 131 controller.i_q_ref = 1; // Current Setpoints
benkatz 25:f5741040c4bb 132 reset_foc(&controller); // Tesets integrators, and other control loop parameters
benkatz 25:f5741040c4bb 133 gpio.enable->write(1); // Enable gate drive
benkatz 25:f5741040c4bb 134 GPIOC->ODR ^= (1 << 5); // Turn on status LED
benkatz 25:f5741040c4bb 135 state_change = 0;
benkatz 23:2adf23ee0305 136 }
benkatz 22:60276ba87ac6 137
benkatz 23:2adf23ee0305 138 void calibrate(void){
benkatz 25:f5741040c4bb 139 gpio.enable->write(1); // Enable gate drive
benkatz 25:f5741040c4bb 140 GPIOC->ODR ^= (1 << 5); // Turn on status LED
benkatz 25:f5741040c4bb 141 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
benkatz 25:f5741040c4bb 142 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
benkatz 25:f5741040c4bb 143 GPIOC->ODR ^= (1 << 5); // Turn off status LED
benkatz 23:2adf23ee0305 144 wait(.2);
benkatz 25:f5741040c4bb 145 gpio.enable->write(0); // Turn off gate drive
benkatz 23:2adf23ee0305 146 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
benkatz 23:2adf23ee0305 147 state_change = 0;
benkatz 23:2adf23ee0305 148
benkatz 23:2adf23ee0305 149 }
benkatz 23:2adf23ee0305 150
benkatz 23:2adf23ee0305 151 void print_encoder(void){
benkatz 23:2adf23ee0305 152 spi.Sample();
benkatz 23:2adf23ee0305 153 wait(.001);
benkatz 23:2adf23ee0305 154 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
benkatz 23:2adf23ee0305 155 wait(.05);
benkatz 22:60276ba87ac6 156 }
benkatz 20:bf9ea5125d52 157
benkatz 23:2adf23ee0305 158 /// Current Sampling Interrupt ///
benkatz 23:2adf23ee0305 159 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
benkatz 2:8724412ad628 160 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 161 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 20:bf9ea5125d52 162 //toggle = 1;
benkatz 23:2adf23ee0305 163
benkatz 23:2adf23ee0305 164 ///Sample current always ///
benkatz 25:f5741040c4bb 165 ADC1->CR2 |= 0x40000000; // Begin sample and conversion
benkatz 22:60276ba87ac6 166 //volatile int delay;
benkatz 20:bf9ea5125d52 167 //for (delay = 0; delay < 55; delay++);
benkatz 25:f5741040c4bb 168 controller.adc2_raw = ADC2->DR; // Read ADC1 and ADC2 Data Registers
benkatz 23:2adf23ee0305 169 controller.adc1_raw = ADC1->DR;
benkatz 23:2adf23ee0305 170 ///
benkatz 20:bf9ea5125d52 171
benkatz 23:2adf23ee0305 172 /// Check state machine state, and run the appropriate function ///
benkatz 23:2adf23ee0305 173 //printf("%d\n\r", state);
benkatz 23:2adf23ee0305 174 switch(state){
benkatz 23:2adf23ee0305 175 case REST_MODE: // Do nothing until
benkatz 23:2adf23ee0305 176 if(state_change){
benkatz 23:2adf23ee0305 177 enter_menu_state();
benkatz 23:2adf23ee0305 178 }
benkatz 23:2adf23ee0305 179 break;
benkatz 22:60276ba87ac6 180
benkatz 23:2adf23ee0305 181 case CALIBRATION_MODE: // Run encoder calibration procedure
benkatz 23:2adf23ee0305 182 if(state_change){
benkatz 23:2adf23ee0305 183 calibrate();
benkatz 23:2adf23ee0305 184 }
benkatz 23:2adf23ee0305 185 break;
benkatz 23:2adf23ee0305 186
benkatz 23:2adf23ee0305 187 case TORQUE_MODE: // Run torque control
benkatz 25:f5741040c4bb 188 if(state_change){
benkatz 25:f5741040c4bb 189 enter_torque_mode();
benkatz 25:f5741040c4bb 190 }
benkatz 23:2adf23ee0305 191 count++;
benkatz 23:2adf23ee0305 192 controller.theta_elec = spi.GetElecPosition();
benkatz 23:2adf23ee0305 193 commutate(&controller, &gpio, controller.theta_elec); // Run current loop
benkatz 23:2adf23ee0305 194 spi.Sample(); // Sample position sensor
benkatz 23:2adf23ee0305 195 if(count > 100){
benkatz 23:2adf23ee0305 196 count = 0;
benkatz 25:f5741040c4bb 197 //readCAN();
benkatz 25:f5741040c4bb 198 //controller.i_q_ref = ((float)(canCmd-1000))/100;
benkatz 23:2adf23ee0305 199 //pc.printf("%f\n\r ", controller.theta_elec);
benkatz 23:2adf23ee0305 200 }
benkatz 23:2adf23ee0305 201 break;
benkatz 23:2adf23ee0305 202
benkatz 23:2adf23ee0305 203 case PD_MODE:
benkatz 23:2adf23ee0305 204 break;
benkatz 23:2adf23ee0305 205 case SETUP_MODE:
benkatz 23:2adf23ee0305 206 if(state_change){
benkatz 24:58c2d7571207 207 enter_setup_state();
benkatz 23:2adf23ee0305 208 }
benkatz 23:2adf23ee0305 209 break;
benkatz 23:2adf23ee0305 210 case ENCODER_MODE:
benkatz 23:2adf23ee0305 211 print_encoder();
benkatz 23:2adf23ee0305 212 break;
benkatz 23:2adf23ee0305 213 }
benkatz 23:2adf23ee0305 214
benkatz 23:2adf23ee0305 215
benkatz 22:60276ba87ac6 216
benkatz 2:8724412ad628 217 }
benkatz 23:2adf23ee0305 218 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 219 }
benkatz 0:4e1c4df6aabd 220
benkatz 25:f5741040c4bb 221
benkatz 24:58c2d7571207 222 char cmd_val[8] = {0};
benkatz 24:58c2d7571207 223 char cmd_id = 0;
benkatz 25:f5741040c4bb 224 char char_count = 0;
benkatz 24:58c2d7571207 225
benkatz 25:f5741040c4bb 226 /// Manage state machine with commands from serial terminal or configurator gui ///
benkatz 25:f5741040c4bb 227 /// Called when data received over serial ///
benkatz 23:2adf23ee0305 228 void serial_interrupt(void){
benkatz 23:2adf23ee0305 229 while(pc.readable()){
benkatz 23:2adf23ee0305 230 char c = pc.getc();
benkatz 25:f5741040c4bb 231 if(c == 27){
benkatz 25:f5741040c4bb 232 state = REST_MODE;
benkatz 25:f5741040c4bb 233 state_change = 1;
benkatz 25:f5741040c4bb 234 char_count = 0;
benkatz 25:f5741040c4bb 235 cmd_id = 0;
benkatz 25:f5741040c4bb 236 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 25:f5741040c4bb 237 }
benkatz 24:58c2d7571207 238 if(state == REST_MODE){
benkatz 23:2adf23ee0305 239 switch (c){
benkatz 23:2adf23ee0305 240 case 'c':
benkatz 23:2adf23ee0305 241 state = CALIBRATION_MODE;
benkatz 23:2adf23ee0305 242 state_change = 1;
benkatz 23:2adf23ee0305 243 break;
benkatz 23:2adf23ee0305 244 case 't':
benkatz 23:2adf23ee0305 245 state = TORQUE_MODE;
benkatz 23:2adf23ee0305 246 state_change = 1;
benkatz 23:2adf23ee0305 247 break;
benkatz 23:2adf23ee0305 248 case 'e':
benkatz 23:2adf23ee0305 249 state = ENCODER_MODE;
benkatz 23:2adf23ee0305 250 state_change = 1;
benkatz 23:2adf23ee0305 251 break;
benkatz 23:2adf23ee0305 252 case 's':
benkatz 23:2adf23ee0305 253 state = SETUP_MODE;
benkatz 23:2adf23ee0305 254 state_change = 1;
benkatz 23:2adf23ee0305 255 break;
benkatz 24:58c2d7571207 256 }
benkatz 24:58c2d7571207 257 }
benkatz 24:58c2d7571207 258 else if(state == SETUP_MODE){
benkatz 25:f5741040c4bb 259 if(c == 13){
benkatz 24:58c2d7571207 260 switch (cmd_id){
benkatz 24:58c2d7571207 261 case 'b':
benkatz 24:58c2d7571207 262 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
benkatz 24:58c2d7571207 263 break;
benkatz 24:58c2d7571207 264 case 'i':
benkatz 24:58c2d7571207 265 CAN_ID = atoi(cmd_val);
benkatz 24:58c2d7571207 266 break;
benkatz 24:58c2d7571207 267 case 'l':
benkatz 24:58c2d7571207 268 TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
benkatz 24:58c2d7571207 269 break;
benkatz 24:58c2d7571207 270 default:
benkatz 24:58c2d7571207 271 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
benkatz 24:58c2d7571207 272 break;
benkatz 24:58c2d7571207 273 }
benkatz 24:58c2d7571207 274
benkatz 24:58c2d7571207 275 if (!prefs.ready()) prefs.open();
benkatz 24:58c2d7571207 276 prefs.flush(); // Write new prefs to flash
benkatz 24:58c2d7571207 277 prefs.close();
benkatz 24:58c2d7571207 278 prefs.load();
benkatz 24:58c2d7571207 279 state_change = 1;
benkatz 24:58c2d7571207 280 char_count = 0;
benkatz 24:58c2d7571207 281 cmd_id = 0;
benkatz 24:58c2d7571207 282 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 24:58c2d7571207 283 }
benkatz 24:58c2d7571207 284 else{
benkatz 24:58c2d7571207 285 if(char_count == 0){cmd_id = c;}
benkatz 24:58c2d7571207 286 else{
benkatz 24:58c2d7571207 287 cmd_val[char_count-1] = c;
benkatz 24:58c2d7571207 288
benkatz 24:58c2d7571207 289 }
benkatz 24:58c2d7571207 290 pc.putc(c);
benkatz 24:58c2d7571207 291 char_count++;
benkatz 23:2adf23ee0305 292 }
benkatz 23:2adf23ee0305 293 }
benkatz 24:58c2d7571207 294 else if (state == ENCODER_MODE){
benkatz 24:58c2d7571207 295 switch (c){
benkatz 24:58c2d7571207 296 case 27:
benkatz 24:58c2d7571207 297 state = REST_MODE;
benkatz 24:58c2d7571207 298 state_change = 1;
benkatz 24:58c2d7571207 299 break;
benkatz 24:58c2d7571207 300 }
benkatz 24:58c2d7571207 301 }
benkatz 24:58c2d7571207 302
benkatz 24:58c2d7571207 303 }
benkatz 22:60276ba87ac6 304 }
benkatz 0:4e1c4df6aabd 305
benkatz 0:4e1c4df6aabd 306 int main() {
benkatz 23:2adf23ee0305 307
benkatz 20:bf9ea5125d52 308 controller.v_bus = V_BUS;
benkatz 22:60276ba87ac6 309 controller.mode = 0;
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 25:f5741040c4bb 332 prefs.load(); // Read flash
benkatz 25:f5741040c4bb 333 spi.SetElecOffset(E_OFFSET); // Set position sensor offset
benkatz 23:2adf23ee0305 334 int lut[128] = {0};
benkatz 23:2adf23ee0305 335 memcpy(&lut, &ENCODER_LUT, sizeof(lut));
benkatz 25:f5741040c4bb 336 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
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 20:bf9ea5125d52 350
benkatz 22:60276ba87ac6 351
benkatz 0:4e1c4df6aabd 352 while(1) {
benkatz 11:c83b18d41e54 353
benkatz 0:4e1c4df6aabd 354 }
benkatz 0:4e1c4df6aabd 355 }