Skip to content

Commit

Permalink
[State_Machine]:change
Browse files Browse the repository at this point in the history
  • Loading branch information
guohua.zhu committed Apr 15, 2021
1 parent 0271720 commit 2021668
Show file tree
Hide file tree
Showing 3 changed files with 23 additions and 20 deletions.
10 changes: 6 additions & 4 deletions Module/State_Machine.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ void StateMachine_Init(void)
total_sum_rotate_turn_cnt = 0;
origin_init_angle_position = d2m_Messege.AngularPosition;
d2m_Messege.MotorTargetPosition = 0.0f;
d2m_Messege.MotorActualPosition = 0.0f;
m2d_Messege.Commond = 0;
}

Expand Down Expand Up @@ -2069,8 +2070,8 @@ void BLDC_RotateTurnControlVelocity(Uint16 phase)
{
target_wait_time = m2d_Messege.TimeInterval[CurrentTurnIndex] * 0.05; // 两次的时间间隔 (s)
target_rote_time = m2d_Messege.RotateTimes [CurrentTurnIndex] * 0.01; // 旋转总运行时间(s)
current_turn_count = m2d_Messege.RotateTurns [CurrentTurnIndex] * 1.875f; // r/s 旋转圈数/s
current_turn_angle_rate = current_turn_count / target_rote_time;
current_turn_count = m2d_Messege.RotateTurns [CurrentTurnIndex] * 1.875f; // 旋转圈数(r)
current_turn_angle_rate = current_turn_count / target_rote_time;// 旋转圈数/s
d2m_Messege.MotorTargetPosition += current_turn_count * TWO_PI;
BLDC_Start();
BLDC_RotateState = WaitRun;
Expand All @@ -2082,7 +2083,7 @@ void BLDC_RotateTurnControlVelocity(Uint16 phase)
break;

case WaitRun:
if (fabs(d2m_Messege.AngularVelocity) > 10)
if (fabs(d2m_Messege.AngularVelocity) > 1)
{
BLDC_RotateState = Running;
}
Expand Down Expand Up @@ -2147,7 +2148,7 @@ void BLDC_RotateTurnControlVelocity(Uint16 phase)
}
else
{
m2d_Messege.TargetAngleVelocity = -10;
m2d_Messege.TargetAngleVelocity = -15;
}
// 近似零位处理
if (m2d_Messege.Commond == APROXMT_ZERO)
Expand All @@ -2161,6 +2162,7 @@ void BLDC_RotateTurnControlVelocity(Uint16 phase)
{
// PositionControllerPID(d2m_Messege.MotorTargetPosition, d2m_Messege.MotorActualPosition, &m2d_Messege.TargetAngleVelocity);
// m2d_Messege.TargetAngleVelocity = _constrain(m2d_Messege.TargetAngleVelocity, -10, 10);
m2d_Messege.TargetAngleVelocity = 0;
}
else
{
Expand Down
12 changes: 6 additions & 6 deletions Module/SteeringEngine.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ void InitEPwmBLDC(void)
EPwm1Regs.AQCTLA.bit.CAD = AQ_CLEAR; // clear actions for EPWM1A
EPwm1Regs.DBCTL.bit.OUT_MODE = DB_FULL_ENABLE; // enable Dead-band module
EPwm1Regs.DBCTL.bit.POLSEL = DB_ACTV_HIC; // Active Hi complementary
EPwm1Regs.DBFED = 30; // FED = 50 TBCLKs
EPwm1Regs.DBRED = 30; // RED = 50 TBCLKs
EPwm1Regs.DBFED = 50; // FED = 50 TBCLKs
EPwm1Regs.DBRED = 50; // RED = 50 TBCLKs

// EPWM Module 2 config
EPwm2Regs.TBPRD = PWM_COUNT; // Period = 900 TBCLK counts
Expand All @@ -78,8 +78,8 @@ void InitEPwmBLDC(void)
EPwm2Regs.AQCTLA.bit.CAD = AQ_CLEAR; // clear actions for EPWM2A
EPwm2Regs.DBCTL.bit.OUT_MODE = DB_FULL_ENABLE; // enable Dead-band module
EPwm2Regs.DBCTL.bit.POLSEL = DB_ACTV_HIC; // Active Hi Complementary
EPwm2Regs.DBFED = 30; // FED = 50 TBCLKs
EPwm2Regs.DBRED = 30; // RED = 50 TBCLKs
EPwm2Regs.DBFED = 50; // FED = 50 TBCLKs
EPwm2Regs.DBRED = 50; // RED = 50 TBCLKs

// EPWM Module 3 config
EPwm3Regs.TBPRD = PWM_COUNT; // Period = 900 TBCLK counts
Expand All @@ -98,8 +98,8 @@ void InitEPwmBLDC(void)
EPwm3Regs.AQCTLA.bit.CAD = AQ_CLEAR; // clear actions for EPWM3A
EPwm3Regs.DBCTL.bit.OUT_MODE = DB_FULL_ENABLE; // enable Dead-band module
EPwm3Regs.DBCTL.bit.POLSEL = DB_ACTV_HIC; // Active Hi complementary
EPwm3Regs.DBFED = 30; // FED = 50 TBCLKs
EPwm3Regs.DBRED = 30; // RED = 50 TBCLKs
EPwm3Regs.DBFED = 50; // FED = 50 TBCLKs
EPwm3Regs.DBRED = 50; // RED = 50 TBCLKs

// Run Time (Note: Example execution of one run-time instant)
//===========================================================
Expand Down
21 changes: 11 additions & 10 deletions Module/Steering_ADC.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,29 +184,30 @@ Uint16 Current_Value_Progress(Uint16 ADC_value)

__interrupt void adc_isr(void)
{
float pid_output_position;
d2m_Messege.MotorDriver_IA = -(int16)(AdcMirror.ADCRESULT0 - 1992) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]
d2m_Messege.MotorDriver_IB = -(int16)(AdcMirror.ADCRESULT1 - 1980) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]
d2m_Messege.MotorDriver_IC = -(int16)(AdcMirror.ADCRESULT2 - 2049) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]
// float pid_output_position;
d2m_Messege.MotorDriver_IA = (int16)(AdcMirror.ADCRESULT0 - 223) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]
d2m_Messege.MotorDriver_IB = (int16)(AdcMirror.ADCRESULT1 - 223) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]
d2m_Messege.MotorDriver_IC = (int16)(AdcMirror.ADCRESULT2 - 223) * 3.32919e-3; // (adc * 3.0 / 4096) * (15 / 3.3) [A]

// update the voltage and current
d2m_Messege.MotorDriverVoltage = (int16)(AdcMirror.ADCRESULT3 - 4) * 0.0076171875; // adc / 4096 * 3 * 10.4
d2m_Messege.MotorDriverVoltage = (int16)(AdcMirror.ADCRESULT3 - 10) * 0.0076171875; // adc / 4096 * 3 * 10.4

// read the position and velocity information
d2m_Messege.FaultState = AD2S1210_ResultRead(&d2m_Messege.AngularPosition, &d2m_Messege.AngularVelocity);

// CurrentProcess(&d2m_Messege.MotorDriver_IA, &d2m_Messege.MotorDriver_IB, &d2m_Messege.MotorDriver_IC, d2m_Messege.ControlPhaseState);


// SpeedControllerPIDParameterSet(m2d_Messege.TargetVd,m2d_Messege.TargetVq);
// 位置控制
// 位置控制
// PositionControllerPID(m2d_Messege.TargetPosition, d2m_Messege.AngularPosition, &pid_output_position);
// SpeedControllerPID(pid_output_position, d2m_Messege.AngularVelocity, &d2m_Messege.V_q);//&pid_output
// 速度控制

// 速度控制
SpeedControllerPID(m2d_Messege.TargetAngleVelocity, d2m_Messege.AngularVelocity, &d2m_Messege.V_q);//&pid_output
d2m_Messege.V_q = _constrain(d2m_Messege.V_q, -20, 20);
d2m_Messege.V_d = 0.01;//m2d_Messege.TargetVd;
d2m_Messege.V_q = _constrain(d2m_Messege.V_q, -14, 14); // 20
d2m_Messege.V_d = 0.01;

// d2m_Messege.V_d = m2d_Messege.TargetVd;
// d2m_Messege.V_q = m2d_Messege.TargetVq;

// InverseParkTransform(d2m_Messege.V_d, d2m_Messege.V_q, m2d_Messege.TargetPosition,
Expand Down

0 comments on commit 2021668

Please sign in to comment.