modified 0511

Dependencies:   mbed-dev FastPWM3

main.cpp

Committer:
WinnieLiu
Date:
21 months ago
Revision:
65:cb3497b549e9
Parent:
64:fd695fb9865b

File content as of revision 65:cb3497b549e9:

/// high-bandwidth 3-phase motor control, for robots
/// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others
/// Hardware documentation can be found at build-its.blogspot.com
/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
/// Version for the TI DRV8323 Everything Chip

// ----- CAN and UART -----
#define REST_MODE 0
#define HALL_CALIBRATE 1 //hall sensor calibration
#define MOTOR_MODE 2
#define SET_ZERO 3

//----- UART only ------
#define SETUP_MODE 4
#define CALIBRATION_MODE 5
#define ENCODER_MODE 6
 
#define VERSION_NUM 3

#define VER2_0 
#define PRINT_UART

 
//float __float_reg[64];                                                          // Floats stored in flash
//float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
//float __float_reg[68]; // add calibrate offset
float __float_reg[69]; // add calibrate speed
//float __float_reg[66];
int __int_reg[257]; //新增calibrate旋轉方向(+1: 逆時針旋轉、-1: 順時針旋轉)
//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
#include "mbed.h"
#include "PositionSensor.h"
#include "structs.h"
#include "foc.h"
#include "calibration.h"
#include "hw_setup.h"
#include "math_ops.h" 
#include "current_controller_config.h"
#include "hw_config.h"
#include "motor_config.h"
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"
#include "user_config.h"
#include "PreferenceWriter.h"
#include "CAN_com.h"
#include "DRV.h"
 
//DigitalOut *CAN_DEBUG = new DigitalOut(PB_15); 
//DigitalOut *GPIO_PID_DEBUG = new DigitalOut(PC_6); 
 
DigitalIn *HALL_IO = new DigitalIn(PC_6); 
 
PreferenceWriter prefs(6);
 
GPIOStruct gpio;
ControllerStruct controller;
ObserverStruct observer;
COMStruct com;
Serial pc(PA_2, PA_3);
 
 
CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
CANMessage   rxMsg;
CANMessage   txMsg;
 
 
SPI drv_spi(PA_7, PA_6, PA_5);
DigitalOut drv_cs(PA_4);
//DigitalOut drv_en_gate(PA_11);
DRV832x drv(&drv_spi, &drv_cs);
 
PositionSensorAM5147 spi(16384, 0.0, NPP);  
 
volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
 
void ChangeParameter(CANMessage msg) ;
//bool complete_changepara = false;
 
/*Hall sensor calibration*/
volatile int hall_input = 1;
volatile int hall_preinput = 1;
volatile float cal_pcmd = 0;
volatile float calibrate_speed = 0.25; // rad/s
volatile float hall_presentpos = 0; //calibrate之前encoder的位置
volatile float hall_in_pos = 0;  //讀到1->0的位置(磁鐵進入hall sensor範圍)
volatile float hall_out_pos = 0; //讀到0->1的位置(磁鐵出hall sensor範圍)
volatile float hall_mid_pos = 0;
volatile float calibrate_offset = 0; //rad
volatile int calibrate_count = 0;
volatile int calibrate_state = 0;
//
 
void onMsgReceived() {
    //static int can_state = 0;
    //msgAvailable = true;
    //printf("%d\n\r", rxMsg.id);
    //CAN_DEBUG->write(1);
    can.read(rxMsg);  
    if((rxMsg.id == CAN_ID)){
        controller.timeout = 0;
        //printf("%X\n\r",0x55);
        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))){
            //printf("can motor\n\r");
            state = MOTOR_MODE;
            state_change = 1;
        }
        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))){
            state = REST_MODE;
            state_change = 1;
            gpio.led->write(0);
        }
        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))){
            spi.ZeroPosition();
            controller.p_des = 0;
            state = SET_ZERO;
        }
//        else if(state == REST_MODE && rxMsg.data[0]==0xFE){
//            printf("Change Parameters\n\r");
//            ChangeParameter(rxMsg);
//            complete_changepara = true;
//        }
        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]==0xFA))){
            //printf("Hall Calibration\n\r");
            calibrate_count = 0;
            calibrate_state = 1; // calibrating
            //step1:讀目前位置
            /*----- convert theta_mech to 0~359.9999deg -----*/
            hall_presentpos = controller.theta_mech;
            cal_pcmd = controller.theta_mech;
            static float _f_cal_round;
            modf(cal_pcmd/(2*PI),&_f_cal_round);
            cal_pcmd = cal_pcmd - _f_cal_round*2*PI;
            if(cal_pcmd < 0) cal_pcmd = cal_pcmd + 2*PI;

            state = HALL_CALIBRATE ;
            state_change = 1;    
        }
        else if(state == MOTOR_MODE){
            unpack_cmd(rxMsg, &controller);
        }
        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state, controller.i_q_ref);
        can.write(txMsg);
        //can_state = can.write(txMsg);
        //CAN_DEBUG->write(0);
        //CAN_DEBUG->write(can_state);
        }
    
}
 
//Use CAN Bus to change parameters
//void ChangeParameter(CANMessage msg){
//    int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
//    int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
//    int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4); 
//    
//    CAN_ID = msg.data[1];
//    MOTOR_KP = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
//    MOTOR_KI = uint_to_float(ki_int, KI_MIN, KI_MAX, 12);
//    MOTOR_KD = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
//    
//    // save to flash
//    if (!prefs.ready()) prefs.open();
//    prefs.flush();                                                  // Write new prefs to flash
//    prefs.close();    
//    prefs.load();
//    /*----- change new pid controller parameter -----*/
//    controller.kp = MOTOR_KP;
//    controller.ki = MOTOR_KI;
//    controller.kd = MOTOR_KD;
//}
 
void enter_menu_state(void){
    drv.disable_gd();
    #ifdef VER2_0
        gpio.enable->write(0);
    #endif
    
    #ifdef PRINT_UART
        printf("\n\r\n\r\n\r");
        printf(" Commands:\n\r");
        wait_us(10);
        printf(" m - Motor Mode\n\r");
        wait_us(10);
        printf(" c - Calibrate Encoder\n\r");
        wait_us(10);
        printf(" s - Setup\n\r");
        wait_us(10);
        printf(" e - Display Encoder\n\r");
        wait_us(10);
        printf(" z - Set Zero Position\n\r");
        wait_us(10);
        printf(" esc - Exit to Menu\n\r");
        wait_us(10);
    #endif
    state_change = 0;
    gpio.led->write(0);
}
 
void enter_setup_state(void){
    printf("\n\r\n\r Configuration Options \n\r\n\n");
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "o", "CAN ID", "0", "127", CAN_ID);
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX);
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX);
    wait_us(10);
    printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
    wait_us(10);
    
    printf(" %-4s %-31s %-5s %-6s %d\n\r", "a", "Calibration Direction", "-1", "1", CALIBRATE_DIR);
    wait_us(10);
    
    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "e", "Calibration offset", "0.0", "143.0", CALIBRATE_OFFSET);
    wait_us(10);
    
    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Calibration Speed", "0.0", "10.0", CALIBRATE_SPEED);
    wait_us(10);
    
    printf(" %-4s %-31s %.2f  %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
    wait_us(10);
    printf(" %-4s %-31s %.2f  %.2f  %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
    wait_us(10);
    printf(" %-4s %-31s %.2f  %.2f   %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD);
    wait_us(10);
    
    printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
    wait_us(10);
    state_change = 0;
    }
    
void enter_torque_mode(void){
    float _f_round, _f_p_des;
    _f_p_des = controller.theta_mech;
    modf(_f_p_des/(2*PI),&_f_round);
    _f_p_des = _f_p_des - _f_round*2*PI;
    if(_f_p_des < 0) _f_p_des = _f_p_des + 2*PI;
    controller.p_des = _f_p_des;
    drv.enable_gd();
    #ifdef VER2_0
        gpio.enable->write(1);
    #endif
    controller.ovp_flag = 0;
    reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
    wait(.001);
    controller.i_d_ref = 0;
    controller.i_q_ref = 0;                                                     // Current Setpoints
    gpio.led->write(1);                                                     // Turn on status LED
    state_change = 0;
    printf("\n\r Entering Motor Mode \n\r");
}
    
void calibrate(void){
    drv.enable_gd();
    #ifdef VER2_0
        gpio.enable->write(1);
    #endif
    gpio.led->write(1);                                                    // Turn on status LED
    order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
    calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
    gpio.led->write(0);;                                                     // Turn off status LED
    wait(.2);
    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
    drv.disable_gd();
    #ifdef VER2_0
        gpio.enable->write(0);
    #endif
     state_change = 0;
    }
    
void print_encoder(void){
    printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
    //printf("%d\n\r", spi.GetRawPosition());
    wait(.001);
    }
 
/// Current Sampling Interrupt ///
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
  if (TIM1->SR & TIM_SR_UIF ) {
        //GPIO_PID_DEBUG->write(1);
 
        ///Sample current always ///
        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
        //volatile int delay;   
        //for (delay = 0; delay < 55; delay++);
 
        spi.Sample(DT);                                                           // sample position sensor
        controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
        controller.adc1_raw = ADC1->DR;
        controller.adc3_raw = ADC3->DR;
        controller.theta_elec = spi.GetElecPosition();
        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
        controller.dtheta_elec = spi.GetElecVelocity();
        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
        
        // Calibration function
        if(calibrate_state == 0 || calibrate_state >= 2 ) ;
        else{
            // read hall sensor
            hall_input = HALL_IO->read() ;
            // calculate new pos
            if((CALIBRATE_DIR == 1 && controller.theta_mech >= hall_presentpos + 2*PI) || (CALIBRATE_DIR == -1 && controller.theta_mech <= hall_presentpos - 2*PI)){
                calibrate_state = 3 ;
                state = REST_MODE ;
                state_change = 1 ;
            }
            else{
                // step3: 馬達正反轉讀hall sensor(1: 沒有感應到磁鐵,0:有感應到磁鐵)
                // 記住1->0瞬間的位置(in_pos),繼續旋轉
                // 記住0->1瞬間的位置(out_pos),停止旋轉,計算in_pos與out_pos的平均值,再讓馬達慢慢轉到位置
                if(hall_input != hall_preinput ) {
                    calibrate_count += 1 ;
                    if(hall_input == 0) hall_in_pos = controller.theta_mech ;
                    else{
                        hall_out_pos = controller.theta_mech ;
                        hall_mid_pos = (hall_in_pos + hall_out_pos)/2.0f ;
                    }
                }

                if(calibrate_count <= 1) cal_pcmd = cal_pcmd + CALIBRATE_DIR*(1.0f/(40000.0f)*CALIBRATE_SPEED ) ;
                else{
                    if(CALIBRATE_DIR == 1 ){
                        if(CALIBRATE_OFFSET == 0){
                            //keep turning
                            if(controller.theta_mech >= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
                            else{//stop
                                cal_pcmd = 0.0f;
                                calibrate_state = 2; //success
                                spi.ZeroPosition();
                                //count = 0;
                                //歸零
                                calibrate_count = 0 ;
                                state = MOTOR_MODE;
                            }
                        }
                        else{
                            if(controller.theta_mech <= hall_mid_pos + CALIBRATE_OFFSET*PI/180)  cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
                            else{ //stop
                                cal_pcmd = 0.0f;
                                calibrate_state = 2; //success
                                spi.ZeroPosition();
                                //歸零
                                calibrate_count = 0 ;
                                state = MOTOR_MODE;
                            }         
                        
                        
                        }
                    }
                    else if(CALIBRATE_DIR == -1){
                        if(CALIBRATE_OFFSET == 0){
                            //keep turning
                            if(controller.theta_mech <= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
                            else{//stop
                                cal_pcmd = 0.0f;
                                calibrate_state = 2; //success
                                spi.ZeroPosition();
                                //歸零
                                calibrate_count = 0 ;
                                state = MOTOR_MODE;
                            }
                        }
                        else{ //calibrate_offset != 0
                            if(controller.theta_mech >= hall_mid_pos - CALIBRATE_OFFSET*PI/180)  cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
                            else{ //stop
                                cal_pcmd = 0.0f;
                                calibrate_state = 2; //success
                                spi.ZeroPosition();
                                //歸零
                                calibrate_count = 0 ;
                                state = MOTOR_MODE;
                            }
                        
                        }
                        

                    
                    }
                }
                
                cal_pcmd = (cal_pcmd>2*PI) ? cal_pcmd-=2*PI : cal_pcmd ;
                cal_pcmd = (cal_pcmd < 0)  ? cal_pcmd+=2*PI : cal_pcmd ;
                controller.p_des = cal_pcmd ;
            }

            hall_preinput = hall_input ;
        }
        
        /// Check state machine state, and run the appropriate function ///
        switch(state){
            case REST_MODE:                                                     // Do nothing
                if(state_change){
                    if(calibrate_state != 2) //success
                        enter_menu_state();
                    else{
                        drv.disable_gd();
                        #ifdef VER2_0
                            gpio.enable->write(0);
                        #endif
                        state_change = 0;
                        gpio.led->write(0);
                    }
                }
                break;
            
            case CALIBRATION_MODE:                                              // Run encoder calibration procedure
                if(state_change){
                    calibrate();
                    }
                break;
             
            case MOTOR_MODE:                                                   // Run torque control
                if(state_change){
                    enter_torque_mode();
                    count = 0;
                    }
                else{
                /*
                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
                    gpio.
                    ->write(0);
                    controller.ovp_flag = 1;
                    state = REST_MODE;
                    state_change = 1;
                    printf("OVP Triggered!\n\r");
                    }
                    */  
 
                    if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                        controller.i_d_ref = 0;
                        controller.i_q_ref = 0;
                        controller.kp = 0;
                        controller.kd = 0;
                        controller.t_ff = 0;                                                                                                                                     
                    } 
 
                    torque_control(&controller);
                    commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop

                    controller.timeout++;
                    count++;
            
                }     
                break;
            case HALL_CALIBRATE:
                    if(state_change){
                        enter_torque_mode();
                        count = 0;
                    }
                    else{
                        if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                            controller.i_d_ref = 0;
                            controller.i_q_ref = 0;
                            controller.kp = 0;
                            controller.kd = 0;
                            controller.t_ff = 0;                                                                                                                                     
                        } 
     
                        torque_control(&controller);
                        commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
    
                        controller.timeout++;
                        count++;
            
                    } 
                break;
            case SETUP_MODE:
                if(state_change){
                    enter_setup_state();
                }
                break;
            case ENCODER_MODE:
                print_encoder();
                break;
                }                 
      }
    TIM1->SR = 0x0;                                                               // reset the status register
  //GPIO_PID_DEBUG->write(0);
}
 
 
char cmd_val[8] = {0};
char cmd_id = 0;
char char_count = 0;
 
/// Manage state machine with commands from serial terminal or configurator gui ///
/// Called when data received over serial ///
void serial_interrupt(void){
    while(pc.readable()){
        char c = pc.getc();
        if(c == 27){
                state = REST_MODE;
                state_change = 1;
                char_count = 0;
                cmd_id = 0;
                gpio.led->write(0);; 
                for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                }
        if(state == REST_MODE){
            switch (c){
                case 'c':
                    state = CALIBRATION_MODE;
                    state_change = 1;
                    break;
                case 'm':
                    state = MOTOR_MODE;
                    state_change = 1;
                    break;
                case 'e':
                    state = ENCODER_MODE;
                    state_change = 1;
                    break;
                case 's':
                    state = SETUP_MODE;
                    state_change = 1;
                    break;
                case 'z':
                    spi.SetMechOffset(0);
                    spi.Sample(DT);
                    wait_us(20);
                    M_OFFSET = spi.GetMechPosition();
                    if (!prefs.ready()) prefs.open();
                        prefs.flush();                                                  // Write new prefs to flash
                        prefs.close();    
                        prefs.load(); 
                    spi.SetMechOffset(M_OFFSET);
                    printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                    
                    break;
                }
                
                }
        else if(state == SETUP_MODE){
            if(c == 13){
                switch (cmd_id){
                    case 'b':
                        I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
                        break;
                    case 'o':
                        CAN_ID = atoi(cmd_val);
                        break;
                    case 'm':
                        CAN_MASTER = atoi(cmd_val);
                        break;
                    case 'l':
                        I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f);
                        break;
                    case 'f':
                        I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f);
                        break;
                    case 't':
                        CAN_TIMEOUT = atoi(cmd_val);
                        break;
                    case 'a':
                        CALIBRATE_DIR = atoi(cmd_val);
                        break;
                    case 'e':
                        CALIBRATE_OFFSET = fmaxf(fminf(atof(cmd_val), 143.0f), 0.0f);
                        break;
                    case 'v':
                        CALIBRATE_SPEED = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f);
                        break;
                    case 'p':
                        MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);;
                        break;
                    case 'i':
                        MOTOR_KI = fmaxf(fminf(atof(cmd_val), KI_MAX), KI_MIN);;
                        break;
                    case 'd':
                        MOTOR_KD = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN);
                        break;
                    default:
                        printf("ASCII: %d\n\r", c);
                        printf("\n\rASCII: %d\n\r", cmd_id);
                        printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
                        break;
                    }
                    
                if (!prefs.ready()) prefs.open();
                prefs.flush();                                                  // Write new prefs to flash
                prefs.close();    
                prefs.load();
                /*----- change new pid controller parameter -----*/
                controller.kp = MOTOR_KP;
                controller.ki = MOTOR_KI;
                controller.kd = MOTOR_KD;
                                                              
                state_change = 1;
                char_count = 0;
                cmd_id = 0;
                for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                }
            else{
                if(char_count == 0){cmd_id = c;}
                else{
                    cmd_val[char_count-1] = c;
                    
                }
                pc.putc(c);
                char_count++;
                }
            }
        else if (state == ENCODER_MODE){
            switch (c){
                case 27:
                    state = REST_MODE;
                    state_change = 1;
                    break;
                    }
            }
        else if (state == MOTOR_MODE){
            switch (c){
                case 'd':
                    controller.i_q_ref = 0;
                    controller.i_d_ref = 0;
                }
            }
            
        }
    }
 
void GPIO_SETUP(){
     //GPIOB->MODER |= 0x40000000;
     //GPIOC->MODER |= 0x0000100f;
     GPIOC->MODER |= 0x00000000;
} 
       
int main() {
    controller.v_bus = V_BUS;
    controller.mode = 0;
    Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
    //DEBUG_GPIO();
    GPIO_SETUP();
    
    wait(.1);
    
    gpio.enable->write(1);
    wait_us(100);
    drv.calibrate();
    wait_us(100);
    drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
    wait_us(100);
    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
    wait_us(100);
    drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
    
    //drv.enable_gd();
    zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
    drv.disable_gd();
    #ifdef VER2_0
        gpio.enable->write(0);
    #endif
 
    wait(.1);
    /*
    gpio.enable->write(1);
    TIM1->CCR3 = 0x708*(1.0f);                        // Write duty cycles
    TIM1->CCR2 = 0x708*(1.0f);
    TIM1->CCR1 = 0x708*(1.0f);
    gpio.enable->write(0);
    */
    reset_foc(&controller);                                                     // Reset current controller
    reset_observer(&observer);                                                 // Reset observer
    TIM1->CR1 ^= TIM_CR1_UDIS;
    //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
    
    wait(.1);
    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
    
    NVIC_SetPriority(CAN1_RX0_IRQn, 3);
                                   // attach 'CAN receive-complete' interrupt handler    
    
    // If preferences haven't been user configured yet, set defaults 
    prefs.load();                                                               // Read flash
    
    can.filter(CAN_ID , 0xFFF, CANStandard, 0);
                                                                    
    txMsg.id = CAN_MASTER;
    txMsg.len = 8;
    rxMsg.len = 8;
    can.attach(&onMsgReceived);  
    
    if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
    if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
    if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
    if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
    if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
    if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
    if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
    if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;}
    if(isnan(MOTOR_KP) || MOTOR_KP==-1){MOTOR_KP = 5;}
    if(isnan(MOTOR_KI) || MOTOR_KI==-1){MOTOR_KI = 0;}
    if(isnan(MOTOR_KD) || MOTOR_KD==-1){MOTOR_KD = 1.25;}
    if(CALIBRATE_DIR != -1 && CALIBRATE_DIR != 1){CALIBRATE_DIR = 1;}
    if(isnan(CALIBRATE_OFFSET) || CALIBRATE_OFFSET==-1){CALIBRATE_OFFSET = 0.0;}
    if(isnan(CALIBRATE_SPEED) || CALIBRATE_SPEED==-1){CALIBRATE_SPEED = 0.25;}
    
    spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
    spi.SetMechOffset(M_OFFSET);
    int lut[128] = {0};
    memcpy(&lut, &ENCODER_LUT, sizeof(lut));
    spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
    init_controller_params(&controller);
 
    pc.baud(115200);                                                            // set serial baud rate
    wait(.01);
    pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
    wait(.01);
    printf("\n\r Debug Info:\n\r");
    printf(" Firmware Version: %d\n\r", VERSION_NUM);
    printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
    printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
    printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
    printf(" CAN ID:  %d\n\r", CAN_ID);
    
    /*----- set controller parameters -----*/
    controller.kp = MOTOR_KP;
    controller.ki = MOTOR_KI;
    controller.kd = MOTOR_KD;
    printf(" PID controller parameters \n\r");
    printf(" KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd);
    printf(" Calibrate Direction: %d\n\r", CALIBRATE_DIR);
    printf(" Calibrate Offset: %f\n\r", CALIBRATE_OFFSET);
    printf(" Calibrate Speed: %f\n\r", CALIBRATE_SPEED);
 
    //printf(" %d\n\r", drv.read_register(DCR));
    //wait_us(100);
    //printf(" %d\n\r", drv.read_register(CSACR));
    //wait_us(100);
    //printf(" %d\n\r", drv.read_register(OCPCR));
    //drv.disable_gd();
    
    pc.attach(&serial_interrupt);                                               // attach serial interrupt
    
    state_change = 1;
    
    // calibrate
    calibrate_state = 0;
 
    int counter = 0;
    while(1) {
//        if(complete_changepara){
//            printf(" %-4s %-31s %d\n\r", "o", "CAN_ID", CAN_ID);
//            printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
//            printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
//            printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD); 
//            complete_changepara = false;
//            state = REST_MODE;
//            state_change = 1;
//        }
        
        
//        printf("Hall_input(PC6): %d\n\r", hall_input);
//        printf("Hall_preinput(PC6): %d\n\r", hall_preinput);
//        printf("calibrate count: %d\n\r",calibrate_count);
//        printf("cal_pcmd: %.4f\n\r",cal_pcmd);
//        printf("pos_in: %f\n\r",hall_in_pos);
//        printf("pos_out: %f\n\r",hall_out_pos);
//        printf("theta_mech: %f\n\r",controller.theta_mech);
//        printf("controller.pdes: %f\n\r", controller.p_des);
//        printf("state: %d\n\r",state);
//        wait(1);
        
        /*
        CAN_DEBUG->write(1);
        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM);
        can_state = can.write(txMsg);
        CAN_DEBUG->write(can_state);
        printf("helloworld\n\r");
        wait(1);
        */
 
        /*
        CAN_DEBUG->write(1);
        wait(0.1);
        CAN_DEBUG->write(0);
        wait(0.1);
        */
        //drv.print_faults();
        //wait(.1);
       //printf("%.4f\n\r", controller.v_bus);
       /*
        if(state == MOTOR_MODE)
        {
            //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
            //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
            //printf("%.3f\n\r", controller.dtheta_mech);
            wait(.002);
        }
        */
 
    }
}