Revision 3:0a2396597e0d, committed 2015-03-02

Comitter:

bwang

Date:

Mon Mar 02 01:24:37 2015 +0000

Parent:

2:8696a62a4077

Commit message:

should turn moter

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/loopdriver.cpp	Mon Mar 02 01:24:37 2015 +0000
@@ -0,0 +1,69 @@
+#include "includes.h"
+#include "core.h"
+#include "sensors.h"
+#include "meta.h"
+#include "lut.h"
+
+Inverter* LoopDriver::_inverter;
+Motor* LoopDriver::_motor;
+User* LoopDriver::_user;
+PidController* LoopDriver::_pid_d;
+PidController* LoopDriver::_pid_q;
+ReferenceSynthesizer* LoopDriver::_reference;
+Modulator* LoopDriver::_modulator;
+
+LoopDriver::LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d, 
+                       PidController *pid_q, Modulator *modulator, float max_phase_current, int update_frequency) {
+    _inverter = inverter;
+    _motor = motor;
+    _user = user;
+    _pid_d = pid_d;
+    _pid_q = pid_q;
+    _reference = new SynchronousReferenceSynthesizer(max_phase_current);;
+    _modulator = modulator;
+    _max_phase_current = max_phase_current;
+    _update_frequency = update_frequency;
+}
+
+void LoopDriver::Start() {
+    _upd_ticker.attach_us(&update, 1000000 / _update_frequency);
+}
+
+void LoopDriver::Clarke(float a, float b, float *alpha, float *beta) {
+    *alpha = a;
+    *beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b;
+}
+
+void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) {
+    float cos = LutCos(theta);
+    float sin = LutSin(theta);
+    *d = alpha * cos + beta * sin;
+    *q = -alpha * sin + beta * cos;
+}
+
+void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) {
+    float cos = LutCos(theta);
+    float sin = LutSin(theta);
+    *alpha = cos * d - sin * q;
+    *beta = sin * d + cos * q;                                     
+}
+
+float LoopDriver::LutSin(float theta) {
+    if (theta < 0.0f) theta += 360.0f;
+    if (theta >= 360.0f) theta -= 360.0f;
+    return sinetab[(int) theta];
+}
+
+float LoopDriver::LutCos(float theta) {
+    return LutSin(90.0f - theta);
+}
+
+void LoopDriver::update() {
+    float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
+    Clarke(_motor->I_a, _motor->I_b, &alpha, &beta);
+    Parke(alpha, beta, _motor->angle, &d, &q);
+    _reference->GetReference(_motor->angle, &ref_d, &ref_q);
+    float vd = _pid_d->Update(ref_d, d);
+    float vq = _pid_q->Update(ref_q, q);
+    InverseParke(vd, vq, _motor->angle, &valpha, &vbeta);
+}

--- a/loopupdater.cpp	Sun Mar 01 10:56:57 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,5 +0,0 @@
-#include "includes.h"
-#include "core.h"
-#include "sensors.h"
-#include "meta.h"
-

--- a/main.cpp	Sun Mar 01 10:56:57 2015 +0000
+++ b/main.cpp	Mon Mar 02 01:24:37 2015 +0000
@@ -12,14 +12,19 @@
     TempSensor *sense_t_inverter = new TempSensor();
     Throttle *throttle = new Throttle(A0, 0.5f, 3.0f);
     
+    PidController *pid_d = new PidController(1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+    PidController *pid_q = new PidController (1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+    
     Motor *motor = new Motor(sense_ia, sense_ib, sense_p, sense_t_motor);
     Inverter *inverter = new Inverter(D6, D13, D3, D8, sense_bus, sense_t_inverter);
     User *user = new User(throttle);
+    Modulator *modulator = new SinusoidalModulator(inverter);
+    StatusUpdater *updater = new StatusUpdater(inverter, motor, user);
+    LoopDriver *driver = new LoopDriver(inverter, motor, user, pid_d, pid_q, modulator, 100.0f, 5000);
     
     motor->Config(4, 20.0f);
-    motor->UpdateState();
+    updater->Config(5000, 10);
     
-    inverter->SetDtcA(0.5f);
-    inverter->SetDtcB(0.5f);
-    inverter->SetDtcC(0.5f);
+    driver->Start();
+    updater->Start();
 }

--- a/meta.h	Sun Mar 01 10:56:57 2015 +0000
+++ b/meta.h	Mon Mar 02 01:24:37 2015 +0000
@@ -29,6 +29,23 @@
     float _out_max, _out_min;
 }; 
 
+class ReferenceSynthesizer {
+public:
+    ReferenceSynthesizer(float max_phase_current) {_max_phase_current = max_phase_current;}
+    virtual void GetReference(float angle, float *ref_d, float *ref_q) {*ref_d = 0; *ref_q = 0;}
+protected:
+    static float LutSin(float theta);
+    static float LutCos(float theta);
+protected:
+    float _max_phase_current;
+};
+
+class SynchronousReferenceSynthesizer : public ReferenceSynthesizer {
+public:
+    SynchronousReferenceSynthesizer(float max_phase_current):ReferenceSynthesizer(max_phase_current) {}
+    virtual void GetReference(float angle, float *ref_d, float *ref_q);
+};
+
 class StatusUpdater {
 public:
     StatusUpdater(Inverter *inverter, Motor *motor, User *user);
@@ -49,20 +66,28 @@
 
 class LoopDriver {
 public:
-    LoopDriver(Inverter *inverter, Motor *motor, User *user, Modulator *modulator, int update_frequency);
+    LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d, PidController *pid_q,  
+               Modulator *modulator, float max_phase_current, int update_frequency);
     void Start();
 private:
-    float Clarke(float a, float b, float c, float *alpha, float *beta);
-    float Parke(float alpha, float beta, float *d, float *q);
-    float InverseParke(float d, float q, float *alpha, float *beta);
+    static void Clarke(float a, float b, float *alpha, float *beta);
+    static void Parke(float alpha, float beta, float theta, float *d, float *q);
+    static void InverseParke(float d, float q, float theta, float *alpha, float *beta);
 private:
     static void update();
 private:
-    Inverter *_inverter;
-    Motor *_motor;
-    User *_user;
-    Modulator *_modulator;
+    static float LutSin(float theta);
+    static float LutCos(float theta);
+private:
+    static Inverter *_inverter;
+    static Motor *_motor;
+    static User *_user;
+    static PidController *_pid_d, *_pid_q;
+    static ReferenceSynthesizer *_reference;
+    static Modulator *_modulator;
     
+    Ticker _upd_ticker;
+    float _max_phase_current;
     int _update_frequency;
 };
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/referencesynthesizers.cpp	Mon Mar 02 01:24:37 2015 +0000
@@ -0,0 +1,18 @@
+#include "includes.h"
+#include "meta.h"
+#include "lut.h"
+
+void SynchronousReferenceSynthesizer::GetReference(float angle, float *ref_d, float *ref_q) {
+    *ref_d = _max_phase_current * LutSin(angle);
+    *ref_q = 0.0f;
+}
+
+float ReferenceSynthesizer::LutSin(float theta) {
+    if (theta < 0.0f) theta += 360.0f;
+    if (theta >= 360.0f) theta -= 360.0f;
+    return sinetab[(int) theta];
+}
+
+float ReferenceSynthesizer::LutCos(float theta) {
+    return LutSin(90.0f - theta);
+}
\ No newline at end of file