Ben Katz
/
Hobbyking_Cheetah_V1
FOC Implementation for putting multirotor motors in robots
CurrentRegulator/CurrentRegulator.cpp@5:51c6560bf624, 2016-03-12 (annotated)
- Committer:
- benkatz
- Date:
- Sat Mar 12 18:45:22 2016 +0000
- Revision:
- 5:51c6560bf624
- Parent:
- 4:c023f7b6f462
- Child:
- 6:4ee1cdc43aa8
Moved InvClarke out of SVM
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 | 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 | 5:51c6560bf624 | 14 | IQ_Ref = .5; |
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 | 1:b8bceb4daed5 | 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 | 4:c023f7b6f462 | 38 | //pc = new Serial(PA_2, PA_3); |
benkatz | 4:c023f7b6f462 | 39 | //pc->baud(115200); |
benkatz | 0:4e1c4df6aabd | 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 | 5:51c6560bf624 | 82 | |
benkatz | 5:51c6560bf624 | 83 | |
benkatz | 0:4e1c4df6aabd | 84 | } |
benkatz | 0:4e1c4df6aabd | 85 | |
benkatz | 0:4e1c4df6aabd | 86 | void CurrentRegulator::SetVoltage(){ |
benkatz | 0:4e1c4df6aabd | 87 | InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta); |
benkatz | 5:51c6560bf624 | 88 | InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C); |
benkatz | 5:51c6560bf624 | 89 | PWM->Update_DTC(V_A, V_B, V_C); |
benkatz | 5:51c6560bf624 | 90 | //PWM->Update_DTC(V_Alpha, V_Beta); |
benkatz | 0:4e1c4df6aabd | 91 | } |
benkatz | 0:4e1c4df6aabd | 92 | |
benkatz | 0:4e1c4df6aabd | 93 | |
benkatz | 0:4e1c4df6aabd | 94 | void CurrentRegulator::Commutate(){ |
benkatz | 4:c023f7b6f462 | 95 | //count += 1; |
benkatz | 0:4e1c4df6aabd | 96 | theta_elec = _PositionSensor->GetElecPosition(); |
benkatz | 1:b8bceb4daed5 | 97 | SampleCurrent(); //Grab most recent current sample |
benkatz | 1:b8bceb4daed5 | 98 | Update(); //Run control loop |
benkatz | 1:b8bceb4daed5 | 99 | SetVoltage(); //Set inverter duty cycles |
benkatz | 4:c023f7b6f462 | 100 | /* |
benkatz | 4:c023f7b6f462 | 101 | if (count==500){ |
benkatz | 4:c023f7b6f462 | 102 | //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 | 103 | pc->putc((unsigned char) (theta_elec*40.0f)); |
benkatz | 4:c023f7b6f462 | 104 | pc->putc((unsigned char) (I_A*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 105 | pc->putc((unsigned char) (I_B*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 106 | pc->putc((unsigned char) (I_Alpha*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 107 | pc->putc((unsigned char) (I_Beta*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 108 | pc->putc((unsigned char) (I_Q*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 109 | pc->putc((unsigned char) (I_D*100.0f+127)); |
benkatz | 4:c023f7b6f462 | 110 | pc->putc((0xff)); |
benkatz | 4:c023f7b6f462 | 111 | count = 0; |
benkatz | 4:c023f7b6f462 | 112 | } |
benkatz | 4:c023f7b6f462 | 113 | */ |
benkatz | 0:4e1c4df6aabd | 114 | } |