66

Dependencies:   mbed-dev-f303 FastPWM3

Files at this revision

API Documentation at this revision

Comitter:
yezhong
Date:
Sun Jun 12 12:31:38 2022 +0000
Parent:
53:89565c1d9115
Commit message:
1

Changed in this revision

CAN/CAN_com.h Show annotated file Show diff for this revision Revisions of this file
Config/user_config.h Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.cpp Show annotated file Show diff for this revision Revisions of this file
PositionSensor/PositionSensor.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/CAN_com.h	Mon Dec 27 02:47:12 2021 +0000
+++ b/CAN/CAN_com.h	Sun Jun 12 12:31:38 2022 +0000
@@ -6,8 +6,8 @@
 #include "mbed.h"
 #include "math_ops.h"
 
- #define P_MIN -12.5f
- #define P_MAX 12.5f
+ #define P_MIN -25.0f      //-12.5f
+ #define P_MAX  25.0f         //12.5f
  #define V_MIN -45.0f
  #define V_MAX 45.0f
  #define KP_MIN 0.0f
--- a/Config/user_config.h	Mon Dec 27 02:47:12 2021 +0000
+++ b/Config/user_config.h	Sun Jun 12 12:31:38 2022 +0000
@@ -10,6 +10,7 @@
 #define TORQUE_LIMIT            __float_reg[3]                                  // Torque limit (current limit = torque_limit/(kt*gear ratio))
 #define THETA_MIN               __float_reg[4]                                  // Minimum position setpoint
 #define THETA_MAX               __float_reg[5]                                  // Maximum position setpoint
+#define guanjieweizhi           __float_reg[8]
 
 
 #define PHASE_ORDER             __int_reg[0]                                    // Phase swapping during calibration
--- a/PositionSensor/PositionSensor.cpp	Mon Dec 27 02:47:12 2021 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun Jun 12 12:31:38 2022 +0000
@@ -53,6 +53,9 @@
     modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
     position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
     MechPosition = position - MechOffset; 
+    //////
+    bb=MechPosition;
+    /////
     float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
     if(elec < 0) elec += 2.0f*PI;
     else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
@@ -114,6 +117,7 @@
     //rotations= quanshu;
     MechOffset = 0;
     //MechOffset = GetMechPosition();
+    //MechOffset =guanjieweizhi;
     Sample(.00025f);
     MechOffset = GetMechPosition();
     }
--- a/PositionSensor/PositionSensor.h	Mon Dec 27 02:47:12 2021 +0000
+++ b/PositionSensor/PositionSensor.h	Sun Jun 12 12:31:38 2022 +0000
@@ -58,6 +58,7 @@
     virtual void WriteLUT(int new_lut[128]);
     //
     int aa;
+    float bb;
     //
 private:
     float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
--- a/main.cpp	Mon Dec 27 02:47:12 2021 +0000
+++ b/main.cpp	Sun Jun 12 12:31:38 2022 +0000
@@ -87,11 +87,12 @@
         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))){
       /*
             quanshu=spi.aa;
+            guanjieweizhi=spi.bb;
          if (!prefs.ready()) prefs.open();
                         prefs.flush();                                                  // Write new prefs to flash
                         prefs.close();    
                         prefs.load(); 
-                        */
+            */            
             spi.ZeroPosition();
             
             }
@@ -196,7 +197,7 @@
     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
+    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");
@@ -217,6 +218,9 @@
         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;
@@ -492,18 +496,18 @@
 
     
     while(1) {
-        if(state == MOTOR_MODE)
+        if(state == MOTOR_MODE ||state == SPEED_MODE||state == Position_MODE)
              {
-          // if(count >= 400){
-//            printf("J: %.3f  Mec: %.3f   Jerr: %.3f   JVerr: %.3f   Kp: %.3f   Kd: %.3f  \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
+          
+          //  printf("J: %.3f  Mec: %.3f   Jerr: %.3f   JVerr: %.3f   Kp: %.3f   Kd: %.3f  \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
             //printf("Jraw:%.3f   J: %.3f  Mec: %.3f   N: %.3d   Nmod: %.3f   Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
 
               //printf("Pdes: %.3f  Vdes: %.3f   Kp: %.3f   Kd: %.3f   Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff);
-              pc.printf("Vdes: %.3f\n\rPrel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.v_des,controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+             // pc.printf("Vdes: %.3f\n\rPrel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.v_des,controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
               //printf("tor: %.3f\n\r",controller.i_q_filt*KT_OUT)
              // count = 0;
-            
-            //}
+             pc.printf("%d\n\r",spi.aa);
+             
 
         }
     }