File content as of revision 48:a74e401a6d84:

/// high-bandwidth 3-phase motor control, for robots
/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, 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

#define REST_MODE 0
#define CALIBRATION_MODE 1
#define MOTOR_MODE 2
#define SETUP_MODE 4
#define ENCODER_MODE 5
#define TEST_MODE 6    // this mode is used to test motor, which allow user control motor's torque directly on the GUI software

#define VERSION_NUM "1.6"


float __float_reg[64];                                                          // Floats stored in flash
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 <string>
 
PreferenceWriter prefs(6);

GPIOStruct gpio;
ControllerStruct controller;
COMStruct com;
ObserverStruct observer;

// Serial pc(PA_2, PA_3);
Serial pc(PC_6, PC_7);

DataPackage datapackage;

CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
CANMessage   rxMsg;
CANMessage   txMsg;

DigitalOut led_debug(PB_1);

PositionSensorAM5147 spi(16384, 0.0, NPP);  

//volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;

volatile float test_tff,test_kp,test_kd,test_pos_des,test_vel_des;
volatile float test_ia,test_ib,test_ic,test_id,test_iq;
volatile float test_mech_angle,test_mech_speed;

void onMsgReceived() {
    //msgAvailable = true;
    
    can.read(rxMsg);  
    if((rxMsg.id == CAN_ID)){
        controller.timeout = 0;
        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))){
            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();
            }
        else if(state == MOTOR_MODE){
            unpack_cmd(rxMsg, &controller);
            }
        // pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q*KT_OUT); //return real torque
//        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.v_bus/2.0f);
        can.write(txMsg);
        }
    
}

void enter_menu_state(void){
    pc.printf("\n\r\n\r\n\r");
    pc.printf(" Commands:\n\r");
    wait_us(10);
    pc.printf(" m - Motor Mode\n\r");
    wait_us(10);
    pc.printf(" c - Calibrate Encoder\n\r");
    wait_us(10);
    pc.printf(" s - Setup\n\r");
    wait_us(10);
    pc.printf(" e - Display Encoder\n\r");
    wait_us(10);
    pc.printf(" z - Set Zero Position\n\r");
    wait_us(10);
    pc.printf(" t - Motor Test Mode\n\r");
    wait_us(10);
    pc.printf(" esc - Exit to Menu\n\r");
    wait_us(10);
    state_change = 0;
    gpio.enable->write(0);
    gpio.led->write(0);
    }

void enter_setup_state(void){
    pc.printf("\n\r\n\r Configuration Options \n\r\n\n");
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
    //pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "1", "100", I_BW);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
    wait_us(10);
    pc.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_test_state(void){
    pc.printf("\n\r\n\r Control Options \n\r\n\n");
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "t", "Torque", "-18", "18", controller.t_ff);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "k", "Stiffness", "0", "500", controller.kp);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "d", "Damping", "0", "5.0", controller.kd);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "p", "Position", "-12.5", "12.5", controller.p_des);
    wait_us(10);
    pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Velocity", "-45", "45", controller.v_des);
    wait_us(10);
    state_change = 0;
    }
    
void enter_torque_mode(void){
    controller.ovp_flag = 0;
    gpio.enable->write(1);                                                      // Enable gate drive
    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;
    pc.printf("\n\r Entering Motor Mode \n\r");
    }
    
void calibrate(void){
    gpio.enable->write(1);                                                      // Enable gate drive
    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);
    gpio.enable->write(0);                                                      // Turn off gate drive
    pc.printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     state_change = 0;
    }
    
void print_encoder(void){
    pc.printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
    wait(.05);
    }

/// Current Sampling Interrupt ///
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
int LLLL = 0;
bool led_flag = 0;
bool flag_test_first = true;

extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
  if (TIM1->SR & TIM_SR_UIF ) {

        ///Sample current always ///
        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
        //volatile int delay;   
        //for (delay = 0; delay < 55; delay++);
        controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
        controller.adc1_raw = ADC1->DR;
        controller.adc3_raw = ADC3->DR;
        spi.Sample(DT);                                                           // sample position sensor
        controller.theta_elec = spi.GetElecPosition();
//        controller.theta_elec = 0;
        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
        controller.dtheta_elec = spi.GetElecVelocity();
//        controller.dtheta_elec = 0;
//        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
         controller.v_bus = 24.0;
        ///
        LLLL ++;
        if(LLLL==10000){
            LLLL=0;
            led_flag = 1-led_flag; 
            led_debug=(led_flag);
            // pc.printf("in interrupt\n\r");
            }
        
        /// Check state machine state, and run the appropriate function ///
        switch(state){
            case REST_MODE:                                                     // Do nothing
                if(state_change){
                    enter_menu_state();
                    flag_test_first = true;
                    }
                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.enable->write(0);
                    controller.ovp_flag = 1;
                    state = REST_MODE;
                    state_change = 1;
                    printf("OVP Triggered!\n\r");
                    }
                    */  
                // controller.t_ff = 1;
                torque_control(&controller);    
                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;
                    } 
                commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                controller.timeout += 1;
                
                /*
                count++;
                if(count == 4000){
                     printf("%.4f\n\r", controller.dtheta_mech);
                     count = 0;
                     }
                     */
                     
            
                }     
                break;
            case SETUP_MODE:
                if(state_change){
                    enter_setup_state();
                }
                break;
            
            case TEST_MODE:
                if(flag_test_first){
                    // enter_test_state();  
                    // enter_torque_mode();  
                    controller.ovp_flag = 0;
                    gpio.enable->write(1);                                                      // Enable gate drive
                    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);
                    flag_test_first = false;                                                     // Turn on status LED
                }
                else{
                     controller.t_ff = test_tff;
                     controller.kp = test_kp;
                     controller.p_des = test_pos_des;
                     controller.kd = test_kd;
                     controller.v_des = test_vel_des;
                     
                     torque_control(&controller);  
                     commutate(&controller, &observer, &gpio, controller.theta_elec); 
                     test_mech_angle = controller.theta_mech;
                     test_mech_speed = controller.dtheta_mech;
                     test_ia = controller.i_a;
                     test_ib = controller.i_b;
                     test_ic = controller.i_c;
//                     test_ia = observer.i_q_est;
//                     test_ib = controller.v_d;
//                     test_ic = controller.v_q;
                     test_id = controller.i_d;
                     test_iq = controller.i_q;
//                     test_iq = observer.i_q_est;
//                     test_id = observer.i_d_est;
//                     test_iq = controller->i_q_filt；
                }
                break;
            
            case ENCODER_MODE:
                print_encoder();
                break;
                }                 
      }
  TIM1->SR = 0x0;                                                               // reset the status register
}


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);
                    pc.printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                    
                    break;
                case 't':
                    // this state is used to test motor through GUI
                    state = TEST_MODE;
                    state_change = 1;
                }
                
                }
        else if(state == SETUP_MODE){
            if(c == 13){
                switch (cmd_id){
                    case 'b':
                        I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
                        //I_BW = fmaxf(fminf(atof(cmd_val), 100.0f), 1.0f);
                        break;
                    case 'i':
                        CAN_ID = atoi(cmd_val);
                        break;
                    case 'm':
                        CAN_MASTER = atoi(cmd_val);
                        break;
                    case 'l':
                        TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
                        break;
                    case 't':
                        CAN_TIMEOUT = atoi(cmd_val);
                        break;
                    default:
                        pc.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();                                              
                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 == TEST_MODE){
            if(c == 13){
                switch(cmd_id){
                    case 't':
                        // controller.t_ff = fmaxf(fminf(atof(cmd_val),  TORQUE_LIMIT), -TORQUE_LIMIT);
                        test_tff = fmaxf(fminf(atof(cmd_val),  TORQUE_LIMIT), -TORQUE_LIMIT);
                        break;
                    case 'k':
                        // controller.kp = fmaxf(fminf(atof(cmd_val),  KP_MAX), KP_MIN);
                        test_kp = fmaxf(fminf(atof(cmd_val),  KP_MAX), KP_MIN);
                        break;
                    case 'd':
                        // controller.kd = fmaxf(fminf(atof(cmd_val),  KD_MAX), KD_MIN);
                        test_kd = fmaxf(fminf(atof(cmd_val),  KD_MAX), KD_MIN);
                        break;
                    case 'p':
                        // controller.p_des = fmaxf(fminf(atof(cmd_val),  P_MAX), P_MIN);
                        test_pos_des = fmaxf(fminf(atof(cmd_val),  P_MAX), P_MIN);
                        break;
                    case 'v':
                        // controller.v_des = fmaxf(fminf(atof(cmd_val),  V_MAX), V_MIN);
                        test_vel_des = fmaxf(fminf(atof(cmd_val),  V_MAX), V_MIN);
                        break;
                    default:
                        // pc.printf("\n\r '%c' Not a valid command prefix\n\r", cmd_id);
                        break;
                    }
                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++;
                }
            }    
        }
    }
       
int main() {
    
    controller.v_bus = V_BUS;
    controller.mode = 0;
    // controller.kd = 0.1;
    Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO

    wait(.1);
    gpio.enable->write(1);
    TIM1->CCR3 = PWM_ARR*(1.0f);                        // Write duty cycles
    TIM1->CCR2 = PWM_ARR*(1.0f);
    TIM1->CCR1 = PWM_ARR*(1.0f);
    zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
    gpio.enable->write(0);
    reset_foc(&controller);                                                     // Reset current controller
    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);
    can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
                                                                    
    txMsg.id = CAN_MASTER;
    txMsg.len = 6;
    rxMsg.len = 8;
    can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler    
    
    prefs.load();                                                               // Read flash
    if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
    if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
    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
    
    // pc.baud(921600);                                                            // set serial baud rate
    pc.baud(460800);
    
    wait(.01);
    //pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
    // pc.printf("aloha, it do is 48v, low cal");
    pc.printf("origin control with current filter 2000hz and current shunt is 1.5mo\n\r");
    pc.printf("vdff_cogging_\n\r");
    wait(.01);
    pc.printf("\n\r Debug Info:\n\r");
    pc.printf(" Firmware Version: %s\n\r", VERSION_NUM);
    pc.printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
    pc.printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
    pc.printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
    pc.printf(" CAN ID:  %d\n\r", CAN_ID);
        
    pc.attach(&serial_interrupt);                                               // attach serial interrupt
    
    // =============================================================================================================
    // add watch dog here
//    Watchdog &watchdog = Watchdog::get_instance();
    // =============================================================================================================
    
    state_change = 1;

    datapackage.info.init_flag1 = 0xffffffff;
    datapackage.info.init_flag2 = 0xfffefdfc;
    datapackage.info.check_num = 0;
    while(1) {
        // pc.printf("hello_wooden");
        // pc.printf("I's still working\n\r");
        // pc.printf("adc1: %d, adc2: %d, adc3: %d",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw);
        if(state == TEST_MODE){
//            datapackage.info.I_a = controller.i_a;
//            datapackage.info.I_b = controller.i_b;
//            datapackage.info.I_c = controller.i_c;
//            datapackage.info.I_q = controller.i_q;
//            datapackage.info.I_d = controller.i_d;
//            datapackage.info.Angle_mech = controller.theta_mech;
//            datapackage.info.Angle_elec = controller.theta_elec;
//            datapackage.info.Control_tff = controller.t_ff;
//            datapackage.info.Control_kp = controller.kp;
//            datapackage.info.Control_p = controller.theta_mech;
//            datapackage.info.Control_kd = controller.kd;
//            datapackage.info.Control_v = controller.dtheta_mech;

            datapackage.info.I_a = test_ia;
            datapackage.info.I_b = test_ib;
            datapackage.info.I_c = test_ic;
            datapackage.info.I_q = test_iq;
            datapackage.info.I_d = test_id;
            datapackage.info.Angle_mech = test_mech_angle;
            datapackage.info.Angle_elec = test_mech_speed;  // actually this return the speed
            datapackage.info.Control_tff = test_tff;
            datapackage.info.Control_kp = test_kp;
            datapackage.info.Control_p = test_pos_des;
            datapackage.info.Control_kd = test_kd;
            datapackage.info.Control_v = test_vel_des;
            
            // pc.write(datapackage.decode, 60);
            // string s(datapackage.decode);
//            pc.printf((const char *)datapackage.decode );wait_us(900);
            
            for(int i=0;i<60;i++){
                pc.putc(datapackage.decode[i]);
                // wait_us(30);
                wait_us(300);
                }
             //wait_us(5000);
            }
        else{
            // can.write(txMsg);
            wait(1.0);
            ;
        }

    }
}