Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

CurrentRegulator/CurrentRegulator.cpp@14:80ce59119d93, 2016-10-31 (annotated)

Committer:
benkatz
Date:
Mon Oct 31 16:48:16 2016 +0000
Revision:
14:80ce59119d93
Parent:
13:a3fa0a31b114 
Misc. changes.  Finally fixed transforms (turns out B and C current measurements were accidentally swapped)

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 12:c473a25f54f7 10 CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, PositionSensor *velocity_sensor, float L, float Kp, float Ki){
benkatz 0:4e1c4df6aabd 11 _Inverter = inverter;
benkatz 11:c83b18d41e54 12 PWM = new SVPWM(inverter, 2.0);
benkatz 0:4e1c4df6aabd 13 _PositionSensor = position_sensor;
benkatz 12:c473a25f54f7 14 _VelocitySensor = velocity_sensor;
benkatz 8:10ae7bc88d6e 15 IQ_Ref = 0;
benkatz 9:d7eb815cb057 16 Q_Max = 40.0f;
benkatz 0:4e1c4df6aabd 17 ID_Ref = 0;
benkatz 0:4e1c4df6aabd 18 V_Q = 0;
benkatz 0:4e1c4df6aabd 19 V_D = 0;
benkatz 0:4e1c4df6aabd 20 V_Alpha = 0;
benkatz 0:4e1c4df6aabd 21 V_Beta = 0;
benkatz 5:51c6560bf624 22 V_A = 0;
benkatz 5:51c6560bf624 23 V_B = 0;
benkatz 5:51c6560bf624 24 V_C = 0;
benkatz 0:4e1c4df6aabd 25 I_Q = 0;
benkatz 0:4e1c4df6aabd 26 I_D = 0;
benkatz 0:4e1c4df6aabd 27 I_A = 0;
benkatz 0:4e1c4df6aabd 28 I_B = 0;
benkatz 0:4e1c4df6aabd 29 I_C = 0;
benkatz 0:4e1c4df6aabd 30 I_Alpha = 0;
benkatz 0:4e1c4df6aabd 31 I_Beta = 0;
benkatz 14:80ce59119d93 32 IQ_Old = 0;
benkatz 14:80ce59119d93 33 ID_Old = 0;
benkatz 8:10ae7bc88d6e 34 count = 0;
benkatz 12:c473a25f54f7 35 _L = L;
benkatz 0:4e1c4df6aabd 36 _Kp = Kp;
benkatz 0:4e1c4df6aabd 37 _Ki = Ki;
benkatz 0:4e1c4df6aabd 38 Q_Integral = 0;
benkatz 0:4e1c4df6aabd 39 D_Integral = 0;
benkatz 0:4e1c4df6aabd 40 Int_Max = .9;
benkatz 2:8724412ad628 41 DTC_Max = .97;
benkatz 0:4e1c4df6aabd 42 //theta_elec = _PositionSensor->GetElecPosition();
benkatz 14:80ce59119d93 43
benkatz 8:10ae7bc88d6e 44
benkatz 0:4e1c4df6aabd 45 }
benkatz 0:4e1c4df6aabd 46
benkatz 4:c023f7b6f462 47 void CurrentRegulator::SendSPI(){
benkatz 4:c023f7b6f462 48
benkatz 4:c023f7b6f462 49
benkatz 4:c023f7b6f462 50 }
benkatz 11:c83b18d41e54 51
benkatz 11:c83b18d41e54 52 float CurrentRegulator::GetQ(){
benkatz 11:c83b18d41e54 53 return I_Q;
benkatz 11:c83b18d41e54 54 }
benkatz 11:c83b18d41e54 55
benkatz 9:d7eb815cb057 56 void CurrentRegulator::Reset(void){
benkatz 9:d7eb815cb057 57 IQ_Ref = 0;
benkatz 9:d7eb815cb057 58 ID_Ref = 0;
benkatz 9:d7eb815cb057 59 V_Q = 0;
benkatz 9:d7eb815cb057 60 V_D = 0;
benkatz 9:d7eb815cb057 61 V_Alpha = 0;
benkatz 9:d7eb815cb057 62 V_Beta = 0;
benkatz 9:d7eb815cb057 63 V_A = 0;
benkatz 9:d7eb815cb057 64 V_B = 0;
benkatz 9:d7eb815cb057 65 V_C = 0;
benkatz 9:d7eb815cb057 66 I_Q = 0;
benkatz 9:d7eb815cb057 67 I_D = 0;
benkatz 9:d7eb815cb057 68 I_A = 0;
benkatz 9:d7eb815cb057 69 I_B = 0;
benkatz 9:d7eb815cb057 70 I_C = 0;
benkatz 9:d7eb815cb057 71 I_Alpha = 0;
benkatz 9:d7eb815cb057 72 I_Beta = 0;
benkatz 9:d7eb815cb057 73 //pc->printf("%f, %f\n\r", Q_Integral, D_Integral);
benkatz 9:d7eb815cb057 74 wait(.03);
benkatz 9:d7eb815cb057 75 Q_Integral = 0;
benkatz 9:d7eb815cb057 76 D_Integral = 0;
benkatz 9:d7eb815cb057 77 }
benkatz 9:d7eb815cb057 78
benkatz 0:4e1c4df6aabd 79 void CurrentRegulator::UpdateRef(float D, float Q){
benkatz 9:d7eb815cb057 80 if(Q>Q_Max){
benkatz 9:d7eb815cb057 81 Q = Q_Max;
benkatz 9:d7eb815cb057 82 }
benkatz 9:d7eb815cb057 83 else if(Q<-Q_Max){
benkatz 9:d7eb815cb057 84 Q = -Q_Max;
benkatz 9:d7eb815cb057 85 }
benkatz 9:d7eb815cb057 86 if(D>Q_Max){
benkatz 9:d7eb815cb057 87 D = Q_Max;
benkatz 9:d7eb815cb057 88 }
benkatz 9:d7eb815cb057 89 else if(D<-Q_Max){
benkatz 9:d7eb815cb057 90 D = -Q_Max;
benkatz 9:d7eb815cb057 91 }
benkatz 0:4e1c4df6aabd 92 IQ_Ref = Q;
benkatz 0:4e1c4df6aabd 93 ID_Ref = D;
benkatz 0:4e1c4df6aabd 94 }
benkatz 0:4e1c4df6aabd 95
benkatz 0:4e1c4df6aabd 96 void CurrentRegulator::SampleCurrent(){
benkatz 0:4e1c4df6aabd 97 _Inverter->GetCurrent(&I_A, &I_B, &I_C);
benkatz 0:4e1c4df6aabd 98 Clarke(I_A, I_B, &I_Alpha, &I_Beta);
benkatz 14:80ce59119d93 99 float ID_Sample, IQ_Sample;
benkatz 14:80ce59119d93 100 //Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
benkatz 14:80ce59119d93 101
benkatz 14:80ce59119d93 102 Park(I_Alpha, I_Beta, theta_elec, &ID_Sample, &IQ_Sample);
benkatz 14:80ce59119d93 103 I_D = .5f*ID_Sample + .5f*ID_Old;
benkatz 14:80ce59119d93 104 I_Q = .5f*IQ_Sample + .5f*IQ_Old;
benkatz 14:80ce59119d93 105 ID_Old = I_D;
benkatz 14:80ce59119d93 106 IQ_Old = I_Q;
benkatz 1:b8bceb4daed5 107 //count += 1;
benkatz 1:b8bceb4daed5 108 //if(count > 10000) {
benkatz 1:b8bceb4daed5 109 // count=0;
benkatz 0:4e1c4df6aabd 110 // printf("I_A: %f I_C: %f I_C: %f\n\r", I_A, I_B, I_C);
benkatz 1:b8bceb4daed5 111 //IQ_Ref = -IQ_Ref;
benkatz 1:b8bceb4daed5 112 // }
benkatz 0:4e1c4df6aabd 113
benkatz 4:c023f7b6f462 114 //DAC->DHR12R1 = (int) (I_Q*490.648f) + 2048;
benkatz 0:4e1c4df6aabd 115 //DAC->DHR12R1 = (int) (I_Alpha*4096.0f) + 2048;
benkatz 0:4e1c4df6aabd 116 }
benkatz 0:4e1c4df6aabd 117
benkatz 0:4e1c4df6aabd 118 void CurrentRegulator::Update(){
benkatz 0:4e1c4df6aabd 119 float Q_Error = IQ_Ref - I_Q;
benkatz 0:4e1c4df6aabd 120 float D_Error = ID_Ref - I_D;
benkatz 13:a3fa0a31b114 121 float w_elec = _VelocitySensor->GetElecVelocity();
benkatz 0:4e1c4df6aabd 122
benkatz 0:4e1c4df6aabd 123 Q_Integral += Q_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 124 D_Integral += D_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 125
benkatz 0:4e1c4df6aabd 126 if (Q_Integral > Int_Max) Q_Integral = Int_Max;
benkatz 2:8724412ad628 127 else if(Q_Integral < -Int_Max) Q_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 128 if (D_Integral > Int_Max) D_Integral = Int_Max;
benkatz 2:8724412ad628 129 else if(D_Integral < -Int_Max) D_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 130
benkatz 0:4e1c4df6aabd 131 V_Q = Q_Integral + _Kp*Q_Error;
benkatz 12:c473a25f54f7 132 //V_Q = V_Q - w_elec*I_D;
benkatz 12:c473a25f54f7 133 V_D = D_Integral + _Kp*D_Error;
benkatz 12:c473a25f54f7 134 //V_D = V_D + w_elec*I_Q; //decoupling needs moar testing
benkatz 0:4e1c4df6aabd 135 }
benkatz 0:4e1c4df6aabd 136
benkatz 0:4e1c4df6aabd 137 void CurrentRegulator::SetVoltage(){
benkatz 0:4e1c4df6aabd 138 InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
benkatz 5:51c6560bf624 139 InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C);
benkatz 5:51c6560bf624 140 PWM->Update_DTC(V_A, V_B, V_C);
benkatz 0:4e1c4df6aabd 141 }
benkatz 0:4e1c4df6aabd 142
benkatz 0:4e1c4df6aabd 143
benkatz 0:4e1c4df6aabd 144 void CurrentRegulator::Commutate(){
benkatz 14:80ce59119d93 145 count += 1;
benkatz 9:d7eb815cb057 146 //GPIOC->ODR = (1 << 4); //Toggle pin for debugging
benkatz 0:4e1c4df6aabd 147 theta_elec = _PositionSensor->GetElecPosition();
benkatz 8:10ae7bc88d6e 148 _PositionSensor->GetMechPosition();
benkatz 1:b8bceb4daed5 149 SampleCurrent(); //Grab most recent current sample
benkatz 1:b8bceb4daed5 150 Update(); //Run control loop
benkatz 1:b8bceb4daed5 151 SetVoltage(); //Set inverter duty cycles
benkatz 9:d7eb815cb057 152 //GPIOC->ODR = (0 << 4); //Toggle pin for debugging
benkatz 7:dc5f27756e02 153
benkatz 14:80ce59119d93 154
benkatz 8:10ae7bc88d6e 155 if (count==1000){
benkatz 14:80ce59119d93 156 //printf("%f\n\r", V_A);
benkatz 4:c023f7b6f462 157 count = 0;
benkatz 4:c023f7b6f462 158 }
benkatz 11:c83b18d41e54 159
benkatz 14:80ce59119d93 160
benkatz 11:c83b18d41e54 161
benkatz 14:80ce59119d93 162
benkatz 11:c83b18d41e54 163 }