Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

CurrentRegulator/CurrentRegulator.cpp@8:10ae7bc88d6e, 2016-04-13 (annotated)

Committer:
benkatz
Date:
Wed Apr 13 04:09:56 2016 +0000
Revision:
8:10ae7bc88d6e
Parent:
7:dc5f27756e02
Child:
9:d7eb815cb057
Multi-turn mechanical position, MA700 position sensing over SPI.  Placeholder torque controller for cogging/ripple compensation;

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 7:dc5f27756e02 12 PWM = new SPWM(inverter, 2.0);
benkatz 0:4e1c4df6aabd 13 _PositionSensor = position_sensor;
benkatz 8:10ae7bc88d6e 14 IQ_Ref = 0;
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 5:51c6560bf624 20 V_A = 0;
benkatz 5:51c6560bf624 21 V_B = 0;
benkatz 5:51c6560bf624 22 V_C = 0;
benkatz 0:4e1c4df6aabd 23 I_Q = 0;
benkatz 0:4e1c4df6aabd 24 I_D = 0;
benkatz 0:4e1c4df6aabd 25 I_A = 0;
benkatz 0:4e1c4df6aabd 26 I_B = 0;
benkatz 0:4e1c4df6aabd 27 I_C = 0;
benkatz 0:4e1c4df6aabd 28 I_Alpha = 0;
benkatz 0:4e1c4df6aabd 29 I_Beta = 0;
benkatz 8:10ae7bc88d6e 30 count = 0;
benkatz 0:4e1c4df6aabd 31 _Kp = Kp;
benkatz 0:4e1c4df6aabd 32 _Ki = Ki;
benkatz 0:4e1c4df6aabd 33 Q_Integral = 0;
benkatz 0:4e1c4df6aabd 34 D_Integral = 0;
benkatz 0:4e1c4df6aabd 35 Int_Max = .9;
benkatz 2:8724412ad628 36 DTC_Max = .97;
benkatz 0:4e1c4df6aabd 37 //theta_elec = _PositionSensor->GetElecPosition();
benkatz 8:10ae7bc88d6e 38 pc = new Serial(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 39 pc->baud(115200);
benkatz 8:10ae7bc88d6e 40
benkatz 0:4e1c4df6aabd 41 }
benkatz 0:4e1c4df6aabd 42
benkatz 4:c023f7b6f462 43 void CurrentRegulator::SendSPI(){
benkatz 4:c023f7b6f462 44
benkatz 4:c023f7b6f462 45
benkatz 4:c023f7b6f462 46 }
benkatz 4:c023f7b6f462 47
benkatz 0:4e1c4df6aabd 48 void CurrentRegulator::UpdateRef(float D, float Q){
benkatz 0:4e1c4df6aabd 49 IQ_Ref = Q;
benkatz 0:4e1c4df6aabd 50 ID_Ref = D;
benkatz 0:4e1c4df6aabd 51 }
benkatz 0:4e1c4df6aabd 52
benkatz 0:4e1c4df6aabd 53 void CurrentRegulator::SampleCurrent(){
benkatz 0:4e1c4df6aabd 54 _Inverter->GetCurrent(&I_A, &I_B, &I_C);
benkatz 0:4e1c4df6aabd 55 Clarke(I_A, I_B, &I_Alpha, &I_Beta);
benkatz 0:4e1c4df6aabd 56 Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
benkatz 1:b8bceb4daed5 57 //count += 1;
benkatz 1:b8bceb4daed5 58 //if(count > 10000) {
benkatz 1:b8bceb4daed5 59 // count=0;
benkatz 0:4e1c4df6aabd 60 // printf("I_A: %f I_C: %f I_C: %f\n\r", I_A, I_B, I_C);
benkatz 1:b8bceb4daed5 61 //IQ_Ref = -IQ_Ref;
benkatz 1:b8bceb4daed5 62 // }
benkatz 0:4e1c4df6aabd 63
benkatz 4:c023f7b6f462 64 //DAC->DHR12R1 = (int) (I_Q*490.648f) + 2048;
benkatz 0:4e1c4df6aabd 65 //DAC->DHR12R1 = (int) (I_Alpha*4096.0f) + 2048;
benkatz 0:4e1c4df6aabd 66 }
benkatz 0:4e1c4df6aabd 67
benkatz 0:4e1c4df6aabd 68 void CurrentRegulator::Update(){
benkatz 0:4e1c4df6aabd 69 float Q_Error = IQ_Ref - I_Q;
benkatz 0:4e1c4df6aabd 70 float D_Error = ID_Ref - I_D;
benkatz 0:4e1c4df6aabd 71
benkatz 0:4e1c4df6aabd 72 Q_Integral += Q_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 73 D_Integral += D_Error*_Ki*_Kp;
benkatz 0:4e1c4df6aabd 74
benkatz 0:4e1c4df6aabd 75 if (Q_Integral > Int_Max) Q_Integral = Int_Max;
benkatz 2:8724412ad628 76 else if(Q_Integral < -Int_Max) Q_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 77 if (D_Integral > Int_Max) D_Integral = Int_Max;
benkatz 2:8724412ad628 78 else if(D_Integral < -Int_Max) D_Integral = -Int_Max;
benkatz 0:4e1c4df6aabd 79
benkatz 0:4e1c4df6aabd 80 V_Q = Q_Integral + _Kp*Q_Error;
benkatz 0:4e1c4df6aabd 81 V_D = D_Integral + _Kp*D_Error;
benkatz 7:dc5f27756e02 82 //V_D = 0;
benkatz 5:51c6560bf624 83
benkatz 5:51c6560bf624 84
benkatz 0:4e1c4df6aabd 85 }
benkatz 0:4e1c4df6aabd 86
benkatz 0:4e1c4df6aabd 87 void CurrentRegulator::SetVoltage(){
benkatz 0:4e1c4df6aabd 88 InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
benkatz 5:51c6560bf624 89 InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C);
benkatz 5:51c6560bf624 90 PWM->Update_DTC(V_A, V_B, V_C);
benkatz 5:51c6560bf624 91 //PWM->Update_DTC(V_Alpha, V_Beta);
benkatz 0:4e1c4df6aabd 92 }
benkatz 0:4e1c4df6aabd 93
benkatz 0:4e1c4df6aabd 94
benkatz 0:4e1c4df6aabd 95 void CurrentRegulator::Commutate(){
benkatz 8:10ae7bc88d6e 96 count += 1;
benkatz 7:dc5f27756e02 97 GPIOC->ODR = (1 << 4); //Toggle pin for debugging
benkatz 0:4e1c4df6aabd 98 theta_elec = _PositionSensor->GetElecPosition();
benkatz 8:10ae7bc88d6e 99 _PositionSensor->GetMechPosition();
benkatz 1:b8bceb4daed5 100 SampleCurrent(); //Grab most recent current sample
benkatz 1:b8bceb4daed5 101 Update(); //Run control loop
benkatz 1:b8bceb4daed5 102 SetVoltage(); //Set inverter duty cycles
benkatz 7:dc5f27756e02 103 GPIOC->ODR = (0 << 4); //Toggle pin for debugging
benkatz 7:dc5f27756e02 104
benkatz 4:c023f7b6f462 105 /*
benkatz 8:10ae7bc88d6e 106 if (count==1000){
benkatz 8:10ae7bc88d6e 107 pc->printf("%f\n\r", _PositionSensor->GetMechPosition());
benkatz 4:c023f7b6f462 108 count = 0;
benkatz 4:c023f7b6f462 109 }
benkatz 4:c023f7b6f462 110 */
benkatz 8:10ae7bc88d6e 111
benkatz 0:4e1c4df6aabd 112 }