Ikhee Jo / T-Motor_AK80_BaseCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev-f303 FastPWM3

CurrentRegulator/CurrentRegulator.cpp@4:c023f7b6f462, 2016-03-12 (annotated)

Committer:
benkatz
Date:
Sat Mar 12 08:04:51 2016 +0000
Revision:
4:c023f7b6f462
Parent:
3:6a0015d88d06
Child:
5:51c6560bf624
serial debugging;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4e1c4df6aabd 1 #include "Transforms.h"
benkatz 0:4e1c4df6aabd 2 #include "CurrentRegulator.h"
benkatz 0:4e1c4df6aabd 3 #include "Inverter.h"
benkatz 0:4e1c4df6aabd 4 #include "SVM.h"
benkatz 0:4e1c4df6aabd 5 #include "Transforms.h"
benkatz 0:4e1c4df6aabd 6 #include "PositionSensor.h"
benkatz 0:4e1c4df6aabd 7
benkatz 0:4e1c4df6aabd 8 using namespace Transforms;
benkatz 0:4e1c4df6aabd 9
benkatz 0:4e1c4df6aabd 10 CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, float Kp, float Ki){
benkatz 0:4e1c4df6aabd 11 _Inverter = inverter;
benkatz 0:4e1c4df6aabd 12 PWM = new SPWM(inverter, 2.0);
benkatz 0:4e1c4df6aabd 13 _PositionSensor = position_sensor;
benkatz 3:6a0015d88d06 14 IQ_Ref = 1;
benkatz 0:4e1c4df6aabd 15 ID_Ref = 0;
benkatz 0:4e1c4df6aabd 16 V_Q = 0;
benkatz 0:4e1c4df6aabd 17 V_D = 0;
benkatz 0:4e1c4df6aabd 18 V_Alpha = 0;
benkatz 0:4e1c4df6aabd 19 V_Beta = 0;
benkatz 0:4e1c4df6aabd 20 I_Q = 0;
benkatz 0:4e1c4df6aabd 21 I_D = 0;
benkatz 0:4e1c4df6aabd 22 I_A = 0;
benkatz 0:4e1c4df6aabd 23 I_B = 0;
benkatz 0:4e1c4df6aabd 24 I_C = 0;
benkatz 0:4e1c4df6aabd 25 I_Alpha = 0;
benkatz 0:4e1c4df6aabd 26 I_Beta = 0;
benkatz 1:b8bceb4daed5 27 //count = 0;
benkatz 0:4e1c4df6aabd 28 _Kp = Kp;
benkatz 0:4e1c4df6aabd 29 _Ki = Ki;
benkatz 0:4e1c4df6aabd 30 Q_Integral = 0;
benkatz 0:4e1c4df6aabd 31 D_Integral = 0;
benkatz 0:4e1c4df6aabd 32 Int_Max = .9;
benkatz 2:8724412ad628 33 DTC_Max = .97;
benkatz 0:4e1c4df6aabd 34 //theta_elec = _PositionSensor->GetElecPosition();
benkatz 4:c023f7b6f462 35 //pc = new Serial(PA_2, PA_3);
benkatz 4:c023f7b6f462 36 //pc->baud(115200);
benkatz 0:4e1c4df6aabd 37
benkatz 0:4e1c4df6aabd 38 }
benkatz 0:4e1c4df6aabd 39
benkatz 4:c023f7b6f462 40 void CurrentRegulator::SendSPI(){
benkatz 4:c023f7b6f462 41
benkatz 4:c023f7b6f462 42
benkatz 4:c023f7b6f462 43 }
benkatz 4:c023f7b6f462 44
benkatz 0:4e1c4df6aabd 45 void CurrentRegulator::UpdateRef(float D, float Q){
benkatz 0:4e1c4df6aabd 46 IQ_Ref = Q;
benkatz 0:4e1c4df6aabd 47 ID_Ref = D;
benkatz 0:4e1c4df6aabd 48 }
benkatz 0:4e1c4df6aabd 49
benkatz 0:4e1c4df6aabd 50 void CurrentRegulator::SampleCurrent(){
benkatz 0:4e1c4df6aabd 51 _Inverter->GetCurrent(&I_A, &I_B, &I_C);
benkatz 0:4e1c4df6aabd 52 Clarke(I_A, I_B, &I_Alpha, &I_Beta);
benkatz 0:4e1c4df6aabd 53 Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
benkatz 1:b8bceb4daed5 54 //count += 1;
benkatz 1:b8bceb4daed5 55 //if(count > 10000) {
benkatz 1:b8bceb4daed5 56 // count=0;
benkatz 0:4e1c4df6aabd 57 // printf("I_A: %f I_C: %f I_C: %f\n\r", I_A, I_B, I_C);
benkatz 1:b8bceb4daed5 58 //IQ_Ref = -IQ_Ref;
benkatz 1:b8bceb4daed5 59 // }
benkatz 0:4e1c4df6aabd 60
benkatz 4:c023f7b6f462 61 //DAC->DHR12R1 = (int) (I_Q*490.648f) + 2048;
benkatz 0:4e1c4df6aabd 62 //DAC->DHR12R1 = (int) (I_Alpha*4096.0f) + 2048;
benkatz 0:4e1c4df6aabd 63 }
benkatz 0:4e1c4df6aabd 64
benkatz 0:4e1c4df6aabd 65 void CurrentRegulator::Update(){
benkatz 0:4e1c4df6aabd 66 float Q_Error = IQ_Ref - I_Q;
benkatz 0:4e1c4df6aabd 67 float D_Error = ID_Ref - I_D;
benkatz 0:4e1c4df6aabd 68
benkatz 0:4e1c4df6aabd 69 Q_Integral += Q_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 70 D_Integral += D_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 71
benkatz 0:4e1c4df6aabd 72 if (Q_Integral > Int_Max) Q_Integral = Int_Max;
benkatz 2:8724412ad628 73 else if(Q_Integral < -Int_Max) Q_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 74 if (D_Integral > Int_Max) D_Integral = Int_Max;
benkatz 2:8724412ad628 75 else if(D_Integral < -Int_Max) D_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 76
benkatz 0:4e1c4df6aabd 77 V_Q = Q_Integral + _Kp*Q_Error;
benkatz 0:4e1c4df6aabd 78 V_D = D_Integral + _Kp*D_Error;
benkatz 0:4e1c4df6aabd 79 }
benkatz 0:4e1c4df6aabd 80
benkatz 0:4e1c4df6aabd 81 void CurrentRegulator::SetVoltage(){
benkatz 0:4e1c4df6aabd 82 InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
benkatz 0:4e1c4df6aabd 83 PWM->Update_DTC(V_Alpha, V_Beta);
benkatz 0:4e1c4df6aabd 84 }
benkatz 0:4e1c4df6aabd 85
benkatz 0:4e1c4df6aabd 86
benkatz 0:4e1c4df6aabd 87 void CurrentRegulator::Commutate(){
benkatz 4:c023f7b6f462 88 //count += 1;
benkatz 0:4e1c4df6aabd 89 theta_elec = _PositionSensor->GetElecPosition();
benkatz 1:b8bceb4daed5 90 SampleCurrent(); //Grab most recent current sample
benkatz 1:b8bceb4daed5 91 Update(); //Run control loop
benkatz 1:b8bceb4daed5 92 SetVoltage(); //Set inverter duty cycles
benkatz 4:c023f7b6f462 93 /*
benkatz 4:c023f7b6f462 94 if (count==500){
benkatz 4:c023f7b6f462 95 //printf("%d %d %d %d\n\r", (int) (I_Q*1000), (int) (I_D*1000), (int) (I_A*1000), int (I_B*1000));
benkatz 4:c023f7b6f462 96 pc->putc((unsigned char) (theta_elec*40.0f));
benkatz 4:c023f7b6f462 97 pc->putc((unsigned char) (I_A*100.0f+127));
benkatz 4:c023f7b6f462 98 pc->putc((unsigned char) (I_B*100.0f+127));
benkatz 4:c023f7b6f462 99 pc->putc((unsigned char) (I_Alpha*100.0f+127));
benkatz 4:c023f7b6f462 100 pc->putc((unsigned char) (I_Beta*100.0f+127));
benkatz 4:c023f7b6f462 101 pc->putc((unsigned char) (I_Q*100.0f+127));
benkatz 4:c023f7b6f462 102 pc->putc((unsigned char) (I_D*100.0f+127));
benkatz 4:c023f7b6f462 103 pc->putc((0xff));
benkatz 4:c023f7b6f462 104 count = 0;
benkatz 4:c023f7b6f462 105 }
benkatz 4:c023f7b6f462 106 */
benkatz 0:4e1c4df6aabd 107 }