Revision 6:99ee0ce47fb2, committed 2015-03-04

Comitter:

nki

Date:

Wed Mar 04 15:33:32 2015 +0000

Parent:

5:ee1e6c84c302

Commit message:

3/4;

Changed in this revision

--- a/config.h	Tue Mar 03 06:28:10 2015 +0000
+++ b/config.h	Wed Mar 04 15:33:32 2015 +0000
@@ -1,6 +1,4 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
 
-#define __DEBUG
-
 #endif
\ No newline at end of file

--- a/core.h	Tue Mar 03 06:28:10 2015 +0000
+++ b/core.h	Wed Mar 04 15:33:32 2015 +0000
@@ -13,17 +13,17 @@
 
 class Motor {
 public:
-    Motor(CurrentSensor *sense_a, CurrentSensor *sense_b, PositionSensor *sense_p, TempSensor *sense_t);
+    Motor(CurrentSensor *sense_c, CurrentSensor *sense_b, PositionSensor *sense_p, TempSensor *sense_t);
     void Config(int num_poles, float kv);
-    float UpdateCurrentA();
+    float UpdateCurrentC();
     float UpdateCurrentB();
     float UpdatePosition();
     float UpdateTemp();
     void UpdateState();
 public:
-    float angle, I_a, I_b, temp;
+    float angle, I_c, I_b, temp;
 private:
-    CurrentSensor *_sense_a,*_sense_b;
+    CurrentSensor *_sense_c,*_sense_b;
     PositionSensor *_sense_p;
     TempSensor *_sense_t;
     int _num_poles;
@@ -43,6 +43,7 @@
 public:
     float dtcA, dtcB, dtcC;
     float v_bus, temp;
+    float va, vb; //debug
 private:
     PwmOut *_pwm_a, *_pwm_b, *_pwm_c;
     DigitalOut *_en;

--- a/currentsensors.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/currentsensors.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -8,7 +8,11 @@
 }
 
 void AnalogCurrentSensor::Zero() {
-    _zero_level = (float) *_in;
+    float mean = 0;
+    for(int i = 1; i<=1000; i++){
+        mean = 0.1f*((float) *_in) + 0.9f*mean;
+    }
+    _zero_level = mean;
 }
 
 float AnalogCurrentSensor::GetCurrent() {

--- a/includes.h	Tue Mar 03 06:28:10 2015 +0000
+++ b/includes.h	Wed Mar 04 15:33:32 2015 +0000
@@ -2,11 +2,14 @@
 #define __INCLUDES_H
 
 #include "mbed.h"
-#include "config.h"
 #include "math.h"
 
-#ifdef __DEBUG
 extern Serial *pc;
-#endif
+extern float test_alpha;
+extern float test_beta;
+
+extern float test_DtcA;
+extern float test_DtcB;
+extern float test_DtcC;
 
 #endif
\ No newline at end of file

--- a/inverter.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/inverter.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -32,19 +32,19 @@
 void Inverter::SetDtcA(float dtc) {
     if (dtc < 0) dtc = 0.0f;
     if (dtc > 1.0f) dtc = 1.0f;
-    *_pwm_a = dtc;
+    *_pwm_a = dtcA = dtc;
 } 
 
 void Inverter::SetDtcB(float dtc) {
     if (dtc < 0) dtc = 0.0f;
     if (dtc > 1.0f) dtc = 1.0f;
-    *_pwm_b = 1.0f - dtc;
+    *_pwm_b = dtcB = 1.0f - dtc;
 } 
 
 void Inverter::SetDtcC(float dtc) {
     if (dtc < 0) dtc = 0.0f;
     if (dtc > 1.0f) dtc = 1.0f;
-    *_pwm_c = dtc;
+    *_pwm_c = dtcC = dtc;
 }
 
 void Inverter::Enable() {

--- a/loopdriver.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/loopdriver.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -30,50 +30,48 @@
     _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::Clarke(float c, float b, float *alpha, float *beta) {
+    *alpha = b;
+    //*beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b;
+    *beta = (b + 2.0f * c)/sqrt(3.0f);
 }
 
 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;
+    *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;                                     
+    //*alpha = cos * d - sin * q;
+    //*beta = -(sin * d + cos * q);  
+    
+    *alpha = cos * 1.0f;
+    *beta = -sin * 1.0f;                                    
 }
 
 float LoopDriver::LutSin(float theta) {
     if (theta < 0.0f) theta += 360.0f;
     if (theta >= 360.0f) theta -= 360.0f;
-    return (sinetab[(int) theta]*2)-1.0f; //shift range 0:1 to -1:1
+    return sinetab[(int) theta] * 2.0f - 1.0f;
 }
 
 float LoopDriver::LutCos(float theta) {
     return LutSin(90.0f - theta);
 }
 
-void LoopDriver::update() {
-    /*
-    _inverter->SetDtcA(LutSin(_motor->angle));
-    _inverter->SetDtcB(LutSin(_motor->angle - 120.0f));
-    _inverter->SetDtcC(LutSin(_motor->angle + 120.0f));
-    */
-    
+void LoopDriver::update() {    
     float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
-    Clarke(_motor->I_a, _motor->I_b, &alpha, &beta);
+    Clarke(_motor->I_c, _motor->I_b, &alpha, &beta);
+    test_alpha = alpha;
+    test_beta = 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);
-    _modulator->Update(valpha, vbeta);
-    
-    
+    _modulator->Update(valpha, vbeta);   
 }

--- a/main.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/main.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -4,26 +4,35 @@
 #include "meta.h"
 
 Serial *pc = new Serial(SERIAL_TX, SERIAL_RX);
+float test_alpha = 0;
+float test_beta = 0;
+
+
+float test_DtcA;
+float test_DtcB;
+float test_DtcC;
 
 int main() {
     pc->baud(115200);
+    pc->printf("%s\n\r", "Init Serial Comm");
+    
     PositionSensor *sense_p = new AnalogHallPositionSensor(A4, A5, 0.249f, 0.497f, 0.231f, 0.499f, 200.0f);
-    CurrentSensor *sense_ia = new AnalogCurrentSensor(A1, 0.01);
+    CurrentSensor *sense_ic = new AnalogCurrentSensor(A1, 0.01);
     CurrentSensor *sense_ib = new AnalogCurrentSensor(A2, 0.01);
     VoltageSensor *sense_bus = new AnalogVoltageSensor(A5, 0.01);
     TempSensor *sense_t_motor = new TempSensor();
     TempSensor *sense_t_inverter = new TempSensor();
     Throttle *throttle = new Throttle(A0, 0.5f, 3.0f);
     
-    PidController *pid_d = new PidController(0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
-    PidController *pid_q = new PidController (0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+    PidController *pid_d = new PidController(0.1f, 0.0f, 0.0f, 1.0f, 0.0f);
+    PidController *pid_q = new PidController (0.1f, 0.0f, 0.0f, 1.0f, 0.0f);
     
-    Motor *motor = new Motor(sense_ia, sense_ib, sense_p, sense_t_motor);
+    Motor *motor = new Motor(sense_ic, 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);
+    LoopDriver *driver = new LoopDriver(inverter, motor, user, pid_d, pid_q, modulator, 1.0f, 5000);
     
     motor->Config(4, 20.0f);
     updater->Config(5000, 10);

--- a/modulators.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/modulators.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -1,8 +1,23 @@
 #include "includes.h"
 #include "meta.h"
 
-void SinusoidalModulator::Update(float vb, float va) {
-    _inverter->SetDtcC(vb);
-    _inverter->SetDtcA(-vb/2.0f + sqrt(3.0f)/2.0f * va);
-    _inverter->SetDtcB(-vb/2.0f - sqrt(3.0f)/2.0f * va);
+void SinusoidalModulator::Update(float va, float vb) {
+    _inverter->va = va;
+    _inverter->vb = vb;
+    
+    _inverter->SetDtcB(va/2.0f + 0.5f);
+    _inverter->SetDtcC((-va / 2.0f - sqrt(3.0f) / 2.0f * vb)/2.0f + 0.5f);
+    _inverter->SetDtcA((-va / 2.0f + sqrt(3.0f) / 2.0f * vb)/2.0f + 0.5f);
+    
+    
+    test_DtcB = va;
+    test_DtcC = -va / 2.0f - sqrt(3.0f) / 2.0f * vb;
+    test_DtcA = -va / 2.0f + sqrt(3.0f) / 2.0f * vb;
+    
+    
+    /*
+    _inverter->SetDtcA(1.0f);
+    _inverter->SetDtcB(0.5f);
+    _inverter->SetDtcC(1.0f);
+    */
 }
\ No newline at end of file

--- a/motor.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/motor.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -2,16 +2,16 @@
 #include "core.h"
 #include "sensors.h"
 
-Motor::Motor(CurrentSensor *sense_a, CurrentSensor *sense_b, PositionSensor *sense_p, TempSensor *sense_t) {
-    _sense_a = sense_a;
+Motor::Motor(CurrentSensor *sense_c, CurrentSensor *sense_b, PositionSensor *sense_p, TempSensor *sense_t) {
+    _sense_c = sense_c;
     _sense_b = sense_b;
     _sense_p = sense_p;
     _sense_t = sense_t;
     UpdateState();
 }
 
-float Motor::UpdateCurrentA() {
-    return I_a = _sense_a->GetCurrent();
+float Motor::UpdateCurrentC() {
+    return I_c = _sense_c->GetCurrent();
 }
 
 float Motor::UpdateCurrentB() {
@@ -27,7 +27,7 @@
 }
 
 void Motor::UpdateState() {
-    UpdateCurrentA();
+    UpdateCurrentC();
     UpdateCurrentB();
     UpdatePosition();
 }

--- a/referencesynthesizers.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/referencesynthesizers.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -10,7 +10,7 @@
 float ReferenceSynthesizer::LutSin(float theta) {
     if (theta < 0.0f) theta += 360.0f;
     if (theta >= 360.0f) theta -= 360.0f;
-    return (sinetab[(int) theta]*2)-1.0f; //shift range 0:1 to -1:1
+    return sinetab[(int) theta] * 2.0f - 1.0f;
 }
 
 float ReferenceSynthesizer::LutCos(float theta) {

--- a/statusupdater.cpp	Tue Mar 03 06:28:10 2015 +0000
+++ b/statusupdater.cpp	Wed Mar 04 15:33:32 2015 +0000
@@ -44,21 +44,25 @@
     int last_slow = 0;
     
     for(;;) {
-        
-        
-        
         if (_time - last_fast > fast_us) {
+            
             _motor->UpdateState();
             _inverter->UpdateVbus();
             last_fast = _time;
+            
+            pc->printf("%f %f %f %f %f\n\r", _motor->angle, _motor->I_c, _motor->I_b, test_alpha, test_beta);
+            //pc->printf("%f %f %f %f\n\r", _motor->angle);
+                                    
         }
         
         if (_time - last_slow > slow_us) {
+            
             _user->UpdateState();
             _motor->UpdateTemp();
             _inverter->UpdateTemp();
             last_slow = _time;
+                                  
+            //pc->printf("%f %f\n\r", _inverter->va, _inverter->vb);
         }
- 
     }
 }
\ No newline at end of file