yezhong yezhong
11
Dependencies: mbed-dev-f303 FastPWM3
Revision 54:4ce8f97be6ae, committed 2022-06-12
- Comitter:
- yezhong
- Date:
- Sun Jun 12 12:31:38 2022 +0000
- Parent:
- 53:89565c1d9115
- Commit message:
- 1
Changed in this revision
diff -r 89565c1d9115 -r 4ce8f97be6ae CAN/CAN_com.h --- 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
diff -r 89565c1d9115 -r 4ce8f97be6ae Config/user_config.h --- 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
diff -r 89565c1d9115 -r 4ce8f97be6ae PositionSensor/PositionSensor.cpp
--- 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();
}
diff -r 89565c1d9115 -r 4ce8f97be6ae PositionSensor/PositionSensor.h
--- 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;
diff -r 89565c1d9115 -r 4ce8f97be6ae main.cpp
--- 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);
+
}
}