1

Dependencies:   mbed-dev-f303 FastPWM3

Revision:
48:1b51771c3647
Parent:
47:55bdc4d5096b
--- a/main.cpp	Sat Dec 07 08:01:06 2019 +0000
+++ b/main.cpp	Fri Feb 07 11:31:37 2020 +0000
@@ -8,13 +8,17 @@
 #define MOTOR_MODE 2
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
-
+#define JOINT_CALIBRATION_MODE 6
+#define J_CALIBRATION_MODE 7
 #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
-
+//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
+int __int_reg[300];  
+int test1;
+int joint_flag=0;
+int stop_sign=0;
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -30,9 +34,12 @@
 #include "user_config.h"
 #include "PreferenceWriter.h"
 #include "CAN_com.h"
+#include "math.h"
+#include "MA700Sensor.h"
+#include "joint_calibration.h"
+PreferenceWriter prefs(6);
+//PreferenceWriter prefs(7);
 
- 
-PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
 ControllerStruct controller;
@@ -40,14 +47,15 @@
 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 count = 0;
 volatile int state = REST_MODE;
@@ -73,7 +81,15 @@
             }
         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);
         }
@@ -88,6 +104,10 @@
     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");
@@ -138,6 +158,8 @@
     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
@@ -145,6 +167,79 @@
      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);     
+    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); 
+    
+    /*************同时设置转子和关节零位置同步****************/
+    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  );
+
+    /*************同时设置转子和关节零位置同步****************/
+    
+    /************Trajectory Planning******************************/
+    
+ 
+       // enter_torque_mode();
+        state=MOTOR_MODE;
+        state_change=1;
+        //enter_torque_mode();
+        count = 0;
+        printf("test\n\r");
+     
+        /*
+        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(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
     wait(.05);
@@ -162,19 +257,37 @@
         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
+        spi.Sample(DT);
+        ma700.Sample(DT);                                                               // 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.init2=controller.theta_mech1 = ma700.GetMechPosition();
+        controller.dtheta_mech1 =ma700.GetMechVelocity();  
+        controller.dtheta_elec1 = ma700.GetElecVelocity();
+        /////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
                     }
                 break;
             
@@ -184,6 +297,23 @@
                     }
                 break;
              
+             case J_CALIBRATION_MODE:                                            // Run encoder calibration procedure
+                if(state_change){
+                     jocalibrate();
+                    
+                    }
+                break;
+             
+             case JOINT_CALIBRATION_MODE:                                            // Run encoder calibration procedure
+                if(state_change){
+                     joint_flag=1;
+                     stop_sign=0;
+                     jointcalibrate();
+                    
+                    }
+                break;
+             
+             
             case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
                     enter_torque_mode();
@@ -200,7 +330,8 @@
                     }
                     */  
 
-                torque_control(&controller);    
+                torque_control(&controller); 
+                /*   
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
@@ -208,6 +339,7 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
+                    */
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                 controller.timeout += 1;
                 
@@ -259,6 +391,16 @@
                     state = CALIBRATION_MODE;
                     state_change = 1;
                     break;
+                    
+                case 't':
+                    state = JOINT_CALIBRATION_MODE;
+                    state_change = 1;
+                    break;
+                 case 'j':
+                    state = J_CALIBRATION_MODE;
+                    state_change = 1;
+                    break;
+                    
                 case 'm':
                     state = MOTOR_MODE;
                     state_change = 1;
@@ -271,17 +413,31 @@
                     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);
                     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  );
+                    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;
                 }
@@ -345,6 +501,7 @@
     
     controller.v_bus = V_BUS;
     controller.mode = 0;
+    controller.sidebct1=0;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
@@ -359,7 +516,7 @@
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
     wait(.1);
-    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
+    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0);                                             // commutation > communication
     
     NVIC_SetPriority(CAN1_RX0_IRQn, 3);
     can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
@@ -368,16 +525,17 @@
     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};
+    int joint[128]={0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
-    spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
-    
+    spi.WriteLUT(lut);  
+    memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
+    spi.WriteLUT(joint);                             
     pc.baud(115200);//pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
@@ -391,10 +549,77 @@
         
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
-    state_change = 1;
+    //state_change = 1;
 
     
-    while(1) {
-
+    while(1) {  
+              wait(.1); 
+              if(state == MOTOR_MODE)
+             {
+              if(joint_flag==1)
+              {
+               if((1.0f/GR)* spi.GetMechPosition()<=0.01)
+               { 
+               //if(stop_sign==0)
+               //{
+                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 
+                {
+                   joint_flag=0; 
+                   controller.v_des =0;
+                   
+                } 
+                */
+                }
+              else if((1.0f/GR)* spi.GetMechPosition()>=(2*PI))
+               { 
+               //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());
+                
+               }  
+             }
+           wait(.1);
+            // printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
+            // printf("%.3d,  %.3d\n\r",joint_flag, stop_sign);
+           
+            //printf("BCT: %.3x   zzz: %.3x   etxy: %.3x \n\r",ma700.Gettest(),ma700.Gettest1(),ma700.Gettest2());
+           // float joint_mech_position=-(controller.theta_mech*360/(2.0f*PI)*GR+controller.theta_mech1*360/(2.0f*PI));
+           // wucha1=(controller.theta_mech-controller.theta_mech1)*360/(2.0f*PI); 
+            //wucha1=controller.theta_mech*360/(2.0f*PI)-joint_mech_position;
+            //wucha+=abs(wucha1); 
+            //printf("M: %.3f J: %.3f E: %.3f  EA: %.3f  \n\r",controller.theta_mech*360/(2.0f*PI),controller.theta_mech1*360/(2.0f*PI),wucha1,float(wucha/i)) ; 
+            // printf("M: %.3f J: %.3f E: %.3f  EA: %.3f  \n\r",controller.theta_mech*360/(2.0f*PI),joint_mech_position,wucha1,float(wucha/i)) ; 
+            //printf("m_position: %.3f\n\r",controller.theta_mech*360/(2.0f*PI)*GR);
+            //printf("j_position: %.3f\n\r",controller.theta_mech1*360/(2.0f*PI));
+            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*57.2957795-controller.theta_mech*2807.49319614;
+          // float j_position=-controller.theta_mech1*57.2957795;
+            printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position);
+            
+            
+            i++;
+            wait(.5);
+            
+        }
+        
+                
     }
 }