Ben Katz
/
Hobbyking_Cheetah_V1
FOC Implementation for putting multirotor motors in robots
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?
User | Revision | Line number | New 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 | } |