this motor config,npp=21,GR= 151

Dependencies:   mbed FastPWM3

main.cpp

Committer:
wyc136
Date:
2021-05-16
Revision:
56:b050b80bab42
Parent:
55:489f01927025

File content as of revision 56:b050b80bab42:

/// 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_TRAJECTORY_MODE 6
#define J_CALIBRATION_MODE 7
#define VERSION_NUM "2.1_RV_50Nm"


float __float_reg[64];                                                          // Floats stored in flash
//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
int __int_reg[300];  
int test1;
int test_jointround_flag=0;
int stop_sign=0;
#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 "math.h"
#include "MA700Sensor.h"
#include "joint_calibration.h"
PreferenceWriter prefs(7);
//PreferenceWriter prefs(7);


GPIOStruct gpio;
ControllerStruct controller;
COMStruct com;
ObserverStruct observer;
Serial pc(PA_2, PA_3);

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

int i=1;//shaorui add
float wucha=0;
float wucha1=0;
PositionSensorAM5147 spi(16384, 0.0, NPP);    //14 bits encoder, 21 NPP
PositionSensorMA700 ma700(16384,0.0,NPP); //shaorui add(12/10)

volatile int count1 = 0;
volatile int state = REST_MODE;
volatile int state_change;
volatile float Joint_init =0; //Joint intial angle
volatile int J_M_flag = 0;     // Joint motor angle combine

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);
            /*
            if(controller.sidebct1!=controller.sidebct)
            {
             controller.sidebct1=controller.sidebct;
             ma700.WriteRegister(&controller);
             }
             */
            }
           
        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
        can.write(txMsg);
        }
    
}

void enter_menu_state(void){
    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(" j - Joint Calibrate Encoder\n\r");
    wait_us(10);
    printf(" t - Joint test 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);
    state_change = 0;
    gpio.enable->write(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 %-5s\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", "i", "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", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
    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("\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){
    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;
    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
    //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
    //j_calibrate(&ma700,&gpio, &controller, &prefs);     
    gpio.led->write(0);;                                                     // Turn off status LED
    wait(.2);
    gpio.enable->write(0);                                                      // Turn off gate drive
    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     state_change = 0;
    }
    
 void jocalibrate(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
    //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
    j_calibrate(&ma700,&gpio, &controller, &prefs);     
 
   /*************同时设置转子和关节零位置同步****************///暂时不需要,标定结束,按z归零
 /*
    spi.SetMechOffset(0);
    ma700.SetMechOffset(0);
    spi.Sample(DT);
    ma700.Sample(DT);
    wait_us(20);
    M_OFFSET = spi.GetMechPosition();
    JOINT_M_OFFSET   =ma700.GetMechPosition();
    if (!prefs.ready()) prefs.open();
    prefs.flush();                                                  // Write new prefs to flash
    prefs.close();    
    prefs.load(); 
    spi.SetMechOffset(M_OFFSET);
    ma700.SetMechOffset(JOINT_M_OFFSET);
    printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
    printf("\n\r  Saved new zero position1:  %.4f\n\r\n\r", JOINT_M_OFFSET);
*/
    /*************同时设置转子和关节零位置同步****************/
    gpio.led->write(0);                                                     // Turn off status LED
    wait(.2);
    gpio.enable->write(0);                                                      // Turn off gate drive
    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     state_change = 0;
    }   
    
void  jointcalibrate(void){
    gpio.enable->write(1);                                                      // Enable gate drive
    gpio.led->write(1);                                                    // Turn on status LED
    //joint_calibrate (&ma700,&spi,&gpio,&controller,&prefs);                 // Perform calibration procedure                      
    gpio.led->write(0);                                                   // Turn off status LED
    wait(.2);
    gpio.enable->write(0); 
    

    
    /************Trajectory Planning******************************/
    
 
       // enter_torque_mode();
        state=MOTOR_MODE;
        state_change=1;
        //enter_torque_mode();
        printf("test\n\r");
        controller.p_des=0;
        controller.v_des = 1.5f;
        controller.kp = 0;
        controller.kd = 5.0f;
        controller.t_ff = 0;
        /*
        if((1.0f/GR)* spi.GetMechPosition()<=(2*PI))
        {
        controller.p_des=0;
        controller.v_des = 2.0f;
        controller.kp = 0;
        controller.kd = 5.0f;
        controller.t_ff = 0;
        wait(.5);
        *
        } 
        
    
************Trajectory Planning*****************************/  
    
                                                         // Turn off gate drive
    printf("\n\r Joint_Calibration complete.  Press 'esc' to return to menu\n\r");
    //state_change = 0;
    }  
    

    
void print_encoder(void){
    printf(" AS5147Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition()*57.2957795, spi.GetElecPosition()*57.2957795, spi.GetRawPosition());
    printf(" MA700:Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", ma700.GetMechPosition()*57.2957795, ma700.GetElecPosition()*57.2957795, ma700.GetRawPosition());
    printf(" Joint raw:  %f                 Joint: %f                Mech: %f\n\r", controller.theta_joint_raw*57.2957795, controller.theta_joint*57.2957795, controller.theta_mech*57.2957795);

    // float m_position=controller.theta_mech*57.2957795;
    // float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR;
    // float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614;
    wait(.5);
    }

/// 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 ) {

        ///Sample current always ///
        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
        //volatile int delay;   
        //for (delay = 0; delay < 55; delay++);
        spi.Sample(DT);
        ma700.Sample(DT); 
        
        controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
        controller.adc1_raw = ADC1->DR;
        controller.adc3_raw = ADC3->DR;
                                                                      // sample position sensor
        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;
        
        
        //////shaorui add for obtaining joint real position
        controller.theta_elec1 = ma700.GetElecPosition();
        controller.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition();
        controller.dtheta_mech1 =-(ma700.GetMechVelocity()+spi.GetMechVelocity());  
        controller.dtheta_elec1 = ma700.GetElecVelocity();
        
        ///hjb add joint angle
        controller.theta_joint_raw= (ma700.GetMechPosition()+spi.GetMechPosition());//the direction of spi.get turn '-' when using RV
        //if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;}  //compensate
       // else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;}
        if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;}
        else if(controller.theta_joint_raw>=2.0f*PI){controller.theta_joint_raw -= 2.0f*PI;}
        
        if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;}   //may be still below 0
        else if(controller.theta_joint_raw>=2.0f*PI){controller.theta_joint_raw -= 2.0f*PI;}

        controller.theta_joint_raw_fil =0.99f*controller.theta_joint_raw_pre + 0.01f*controller.theta_joint_raw ;//filter
        controller.theta_joint_raw_pre =controller.theta_joint_raw_fil;
        
        //"rad 0~2pi" ("deg" =-(controller.theta_mech1+controller.theta_mech)*2807.49319614;
        //int Ncycle=0;
        //float Ncycle_mod = 0.0f;
        //float Mech_mod  = 0.0f;
        controller.Ncycle = controller.theta_joint_raw/(2.0f*PI/GR);
        controller.Ncycle_mod = fmod(controller.theta_joint_raw,2.0f*PI/GR);
        controller.Mech_mod = fmod(spi.GetMechPosition(),2.0f*PI);
        
        if(controller.Mech_mod<0){controller.Mech_mod += 2.0f*PI;}
        else if(controller.Mech_mod>=2.0f*PI){controller.Mech_mod -= 2.0f*PI;}

        controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR;  //In the real use, should turn to the theta_mech
        if(controller.theta_joint<0){controller.theta_joint += 2.0f*PI;}
        else if(controller.theta_joint>=2.0f*PI){controller.theta_joint -= 2.0f*PI;}
        if(J_M_flag == 0)
          {
            if(0<abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2)
           {
               if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<2)
                {
                Joint_init =  controller.theta_joint- (1.0f/GR)*spi.GetMechPosition();
                controller.theta_mech = controller.theta_joint;////controller.theta_joint;                                 // easy way to use, whether available in the shock?
                J_M_flag = 1;
                }
              }
            controller.theta_mech = controller.theta_joint_raw;
            
           }
        else if(J_M_flag == 1)
        {
            controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition(); 
         
            
            }
        
        
        /////shaorui end//////////////////
        /*
        controller.c++;
        if(controller.c>=20000)
        {
        controller.cha=controller.init2-controller.init1;
        controller.init1=controller.init2;
        controller.c=0;
        printf("position: %.3f  \n\r", controller.cha*360/(2.0f*PI));
        }
        */
        /// Check state machine state, and run the appropriate function ///
        switch(state){
            case REST_MODE:                                                     // Do nothing
                if(state_change){
                    enter_menu_state();
                    wucha=0 ; //shaorui add
                    test_jointround_flag=0;
                    }
                break;
            
            case CALIBRATION_MODE:                                              // Run encoder calibration procedure
                if(state_change){
                    calibrate();
                    }
                break;
             
             case J_CALIBRATION_MODE:                                            // Run encoder calibration procedure
                if(state_change){
                     jocalibrate();
                    
                    }
                break;
             
             case TEST_TRAJECTORY_MODE:                                            // Run encoder calibration procedure
                if(state_change){
                     test_jointround_flag=1;
                     //J_M_flag = 0;
                     stop_sign=0;
                     jointcalibrate();
                    
                    }
                break;
             
             
            case MOTOR_MODE:                                                   // Run torque control
                if(state_change){
                    enter_torque_mode();
                    count1 = 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");
                    }
                    */  
                count1++;
                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;
                
                
               
                /*
                if(count1 == 4000){
                     printf("%.4f\n\r", controller.dtheta_mech);
                     count1 = 0;
                     }
                     */
                     
            
                }     
                break;
            case SETUP_MODE:
                if(state_change){
                    enter_setup_state();
                }
                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;
                test_jointround_flag=0;
                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 't':
                    state = TEST_TRAJECTORY_MODE;
                    state_change = 1;
                    break;
                 case 'j':
                    state = J_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);
                    ma700.SetMechOffset(0);
                    spi.Sample(DT);
                    ma700.Sample(DT);
                    wait_us(20);
                    M_OFFSET = spi.GetMechPosition();
                    JOINT_M_OFFSET = ma700.GetMechPosition();
                    if (!prefs.ready()) prefs.open();
                        prefs.flush();                                                  // Write new prefs to flash
                        prefs.close();    
                        prefs.load(); 
                    spi.SetMechOffset(M_OFFSET);
                    ma700.SetMechOffset(JOINT_M_OFFSET);
                    J_M_flag=0;
                    printf("\n\r  Saved new M-J J-J zero position:  %.4f %.4f\n\r\n\r", M_OFFSET, JOINT_M_OFFSET);
                  /*
                    for(int i=0;i<300;i++)
                    {
                      printf("%.3d   %.3d\n\r",i,__int_reg[i] ); 
                    }
                    for(int j=0;j<64;j++)
                    {
                    printf("%.3d   %.3f\n\r",j,__float_reg[j] ); 
                    }
                   */
                    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 '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:
                        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;
                    }
            }
            
        }
    }
       
int main() {
    
    controller.v_bus = V_BUS;
    controller.mode = 0;
    controller.sidebct1=0;
    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
    reset_foc(&controller);                                                     // Reset current controller
    reset_observer(&observer);                                                 // Reset observer
    wait(.1);
    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0);                                             // commutation > communication
    
    NVIC_SetPriority(CAN1_RX0_IRQn, 3);
        // 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 = 6;
    rxMsg.len = 8;
    can.attach(&onMsgReceived);  
    
    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);
    
                                                       // Set potision sensor nonlinearity lookup table
    init_controller_params(&controller);
    //pc.baud(460800);//pc.baud(921600); 
    pc.baud(115000);
    //=====benk 2020 last version===//
    
    spi.Sample(1.0f);
     printf("MP: %.3f\n\r",spi.GetMechPosition());
    if(spi.GetMechPosition() > 2.0f*PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);}        // now zeroes to +- 30 degrees about nominal, independent of rollover point
    else if (spi.GetMechPosition() < 0){spi.SetMechOffset(M_OFFSET-2.0f*PI);}
    //=====benk 2020 last version===//
    
    if(isnan(JOINT_E_OFFSET)){JOINT_E_OFFSET = 0.0f;}
    if(isnan(JOINT_M_OFFSET)){JOINT_M_OFFSET = 0.0f;}
    ma700.SetElecOffset(JOINT_E_OFFSET);                                                // Set position sensor offset
    ma700.SetMechOffset(JOINT_M_OFFSET);
    //=====benk 2020 last version===//
    ma700.Sample(1.0f);
    printf("JP: %.3f\n\r",ma700.GetMechPosition());
    if(ma700.GetMechPosition() > 2.0f*PI){ma700.SetMechOffset(JOINT_M_OFFSET+2.0f*PI);}        // now zeroes to +- 30 degrees about nominal, independent of rollover point
    else if (ma700.GetMechPosition() < 0){ma700.SetMechOffset(JOINT_M_OFFSET-2.0f*PI);}
    
    spi.Sample(1.0f);
    ma700.Sample(1.0f);
    printf("MP: %.3f   JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());

    //=====benk 2020 last version===//
    int lut[128] = {0};
    int joint[128]={0};                                                         //shaorui
    memcpy(&lut, &ENCODER_LUT, sizeof(lut));
    spi.WriteLUT(lut);  
    memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
    ma700.WriteLUT(joint);                             
                                                               // set serial baud rate
    wait(.01);
        printf("MP: %.3f   JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());

    pc.printf("\n\r\n\r QXSLAB Joint\n\r\n\r");
    wait(.01);
    printf("\n\r Debug Info:\n\r");
    printf(" Firmware Version: %s\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*57.2957795);
    printf(" Output Zero Position:  %.4f\n\r", M_OFFSET*57.2957795);
    printf(" CAN ID:  %d\n\r", CAN_ID);
    
    controller.theta_joint_raw_pre=-(ma700.GetMechPosition()+spi.GetMechPosition());    //hjb added, for joint encoder filter    
    pc.attach(&serial_interrupt);                                               // attach serial interrupt
    J_M_flag = 0;
    //state_change = 1;

    
    while(1) {  
             // wait(.1); 
              if(state == MOTOR_MODE)
             {
              
              
              
               if(controller.theta_joint_raw*57.2957795<=1)
               { 
               
                controller.v_des = 0;
                wait(1);
                controller.p_des=0;
                controller.v_des = 1.5f;
                controller.kp = 0;
                controller.kd = 5.0f;
                controller.t_ff = 0;
                wait(.5); 
                }
               
              else if(controller.theta_joint_raw*57.2957795>=(359))
               { 
               //stop_sign=1;
                controller.v_des = 0;
                wait(1);
                controller.p_des=0;
                controller.v_des = -1.5f;
                controller.kp = 0;
                controller.kd = 5.0f;
                controller.t_ff = 0;
                wait(.5);
                printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
                
               } 
               //printf("test position:real mebh SPI %.3f  OUT %.3f\n\r",(1.0f/GR)* spi.GetMechPosition(),ma700.GetMechPosition());//wyc 2021.05.14
           
           if(count1 >= 40000){        
          //  printf("J_init: %.3f Pref: %.3f  preal: %.3f   JM: %.3d one: %.3f  two: %.3f\n\r",Joint_init*57.2957795,(1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_mech*57.2957795,J_M_flag ,controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR,(controller.theta_joint- controller.theta_joint_raw)*57.2957795);
            printf("Pref: %.3f  qian: %.3f  hou: %.3f  M: %.3d \n\r", (1.0f/GR)*spi.GetMechPosition()*57.2957795,controller.theta_joint_raw*57.2957795,controller.theta_mech*57.2957795,J_M_flag);
            // printf("v: %.3f  v1: %.3f   \n\r",controller.dtheta_mech*57.2957795,controller.dtheta_mech1*57.2957795);
              count1 = 0;
              }
              
             i++;
        }
 }       
}