stock mbed AnalogReads current loop closed and working

Dependencies:   mbed

Fork of priustroller by Bayley Wang

Files at this revision

API Documentation at this revision

Comitter:
nki
Date:
Thu Apr 16 04:08:47 2015 +0000
Parent:
35:83cf9564bd0c
Commit message:
running code, buggy at high current

Changed in this revision

callbacks.cpp Show annotated file Show diff for this revision Revisions of this file
context.cpp Show annotated file Show diff for this revision Revisions of this file
context.h Show annotated file Show diff for this revision Revisions of this file
core/core.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
sensors/throttle.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 83cf9564bd0c -r 11766b5da6ed callbacks.cpp
--- a/callbacks.cpp	Mon Mar 16 15:40:07 2015 +0000
+++ b/callbacks.cpp	Thu Apr 16 04:08:47 2015 +0000
@@ -23,7 +23,7 @@
     //The necessary changes were analytically determined by observing that a postive vd command corresponded to a reading of negative q axis torque
     //and that a positive vq command produced a reading of positive d axis torque.  
     //This may be the final step in aligning the handedness of the reference and the output.
-
+    
     c->reference->GetReference(angle, c->user->throttle, &ref_d, &ref_q);
     
     dbg_ref_d = ref_d;
@@ -31,6 +31,19 @@
     dbg_loop_d = vd = c->pid_d->Update(ref_d, d_filtered);
     dbg_loop_q = vq = c->pid_q->Update(ref_q, q_filtered);
     
+    /*
+    
+    if(c->user->throttle <= 0.1f){
+        float ramp = (c->user->throttle*10.0f)*(c->user->throttle*10.0f);
+        vd = ramp*vd;
+        vq = ramp*vq;
+        }
+    
+    */
+        
+    dbg_loop_d = vd;
+    dbg_loop_q = vq;
+        
     InverseParke(vd, vq, angle, &valpha, &vbeta);
     
     dbg_valpha = valpha;
@@ -41,14 +54,16 @@
 
 void slow(Context *c) {
     c->user->UpdateThrottle();
+    //c->user->throttle = c->filter_th->Update(c->user->throttle);
 }
 
 void debug(Context *c) {
-    //c->serial->printf("%f %f %f %f %f %f\n\r", dbg_d_filtered, dbg_q_filtered, dbg_ref_d, dbg_ref_q, dbg_loop_d, dbg_loop_q);
-    //c->serial->printf("%f %f %f %f\n\r", dbg_d_filtered, dbg_q_filtered);
+    c->serial->printf("%f %f %f %f %f %f\n\r", dbg_d_filtered, dbg_q_filtered, dbg_ref_d, dbg_ref_q, dbg_loop_d, dbg_loop_q);
+    //c->serial->printf("%f\n\r", dbg_angle);
+    //c->serial->printf("%f\n\r", c->user->throttle);
 }
 
 void log(Context *c) {
-    //c->debugger->Write(0, dbg_d_filtered);
+    //c->debugger->Write(0, dbg_angle);
     //c->debugger->Write(1, dbg_q_filtered);
 }
\ No newline at end of file
diff -r 83cf9564bd0c -r 11766b5da6ed context.cpp
--- a/context.cpp	Mon Mar 16 15:40:07 2015 +0000
+++ b/context.cpp	Thu Apr 16 04:08:47 2015 +0000
@@ -45,10 +45,11 @@
     _qpidmin = pidmin;
 }
 
-void Context::ConfigureThrottle(PinName throttle_pin, float min, float max) {
+void Context::ConfigureThrottle(PinName throttle_pin, float min, float max, float th_filter_strength) {
     _throttle_pin = throttle_pin;
     _min = min;
     _max = max;
+    _th_filter_strength = th_filter_strength;
 }
 
 void Context::ConfigurePositionSensor(PinName pos_a_pin, PinName pos_b_pin, float cal1_a, float cal2_a, float cal1_b, float cal2_b, float offset) {
@@ -108,6 +109,8 @@
     reference = new SynchronousReferenceSynthesizer(_max_current);
     filter_d = new MeanFilter(_filter_strength);
     filter_q = new MeanFilter(_filter_strength);
+    filter_th = new MeanFilter(_th_filter_strength);
+    
     
     serial = new Serial(USBTX, USBRX);
     serial->baud(115200);
diff -r 83cf9564bd0c -r 11766b5da6ed context.h
--- a/context.h	Mon Mar 16 15:40:07 2015 +0000
+++ b/context.h	Thu Apr 16 04:08:47 2015 +0000
@@ -18,7 +18,7 @@
     void ConfigureCurrentSensors(PinName ib_pin, PinName ic_pin, float scale, float filter_strength);
     void ConfigureIdPidController(float ki, float kp, float kd, float pidmin, float pidmax);
     void ConfigureIqPidController(float ki, float kp, float kd, float pidmin, float pidmax);
-    void ConfigureThrottle(PinName throttle_pin, float min, float max);
+    void ConfigureThrottle(PinName throttle_pin, float min, float max, float th_filter_strength);
     void ConfigurePositionSensor(PinName pos_a_pin, PinName pos_b_pin, float cal1_a, float cal2_a, float cal1_b, float cal2_b, float offset);
     void ConfigureReference(float max_current);
     void ConfigureDebugger(int debugger_channels, int debugger_size);
@@ -37,7 +37,7 @@
     Modulator *modulator;
     Serial *serial;
     ReferenceSynthesizer *reference;
-    LtiFilter *filter_d, *filter_q;
+    LtiFilter *filter_d, *filter_q, *filter_th;
     BufferedDebugger *debugger;
 private:
     void InitData();
@@ -46,7 +46,7 @@
 private:
     PinName _oa, _ob, _oc, _ib_pin, _ic_pin, _throttle_pin, _pos_a_pin, _pos_b_pin, _en;
     float _dki, _dkp, _dkd, _qki, _qkp, _qkd, _min, _max, _cal1_a, _cal2_a, 
-          _cal1_b, _cal2_b, _scale, _offset, _max_current, _filter_strength;
+          _cal1_b, _cal2_b, _scale, _offset, _max_current, _filter_strength, _th_filter_strength;
     int _debugger_channels, _debugger_size;
     float _dpidmax, _dpidmin, _qpidmax, _qpidmin;
     void (*_callbacks[16])(Context *);
diff -r 83cf9564bd0c -r 11766b5da6ed core/core.h
--- a/core/core.h	Mon Mar 16 15:40:07 2015 +0000
+++ b/core/core.h	Thu Apr 16 04:08:47 2015 +0000
@@ -47,7 +47,7 @@
 class User {
 public:
     User(Throttle *throttle) {_throttle = throttle;}
-    void UpdateThrottle() {throttle = _throttle->GetThrottle();}
+    void UpdateThrottle() {throttle = _throttle->GetThrottle();} 
 public:
     float throttle;
 private:
diff -r 83cf9564bd0c -r 11766b5da6ed main.cpp
--- a/main.cpp	Mon Mar 16 15:40:07 2015 +0000
+++ b/main.cpp	Thu Apr 16 04:08:47 2015 +0000
@@ -9,14 +9,14 @@
 
 int main() { 
     Context *context = new Context();
-    context->ConfigureOutputs(D6, D13, D3, D8);
+    context->ConfigureOutputs(D13, D3, D6, D2);
     context->ConfigureCurrentSensors(A1, A2, 0.01f, 0.7f);
-    context->ConfigureIdPidController(0.001f, 0.0f, 0.0f, 1.0f, -1.0f);
-    context->ConfigureIqPidController(0.001f, 0.0f, 0.0f, 1.0f, -1.0f);
-    context->ConfigureThrottle(A0, 0.8f, 3.0f);
-    context->ConfigurePositionSensor(A4, A5, 0.249f, 0.497f, 0.231f, 0.499f, 205.0f);
-    context->ConfigureReference(3.0f);  // max phase current
-    context->ConfigureDebugger(2, 2000);
+    context->ConfigureIdPidController(0.0008f, 0.0f, 0.0f, 1.0f, -1.0f);
+    context->ConfigureIqPidController(0.0008f, 0.0f, 0.0f, 1.0f, -1.0f);
+    context->ConfigureThrottle(A0, 0.9f, 2.5f, 0.8f);  //last term is LPF strength
+    context->ConfigurePositionSensor(A3, A4, 0.366f, 0.655f, 0.355f, 0.626f, 205.0f);
+    context->ConfigureReference(80.0f);  // max phase current
+    context->ConfigureDebugger(1, 2000);
     context->AttachCallBack(&fast, 5000);
     context->AttachCallBack(&slow, 10);
     context->AttachCallBack(&debug, 10);
diff -r 83cf9564bd0c -r 11766b5da6ed sensors/throttle.cpp
--- a/sensors/throttle.cpp	Mon Mar 16 15:40:07 2015 +0000
+++ b/sensors/throttle.cpp	Thu Apr 16 04:08:47 2015 +0000
@@ -1,5 +1,6 @@
 #include "includes.h"
 #include "sensors.h"
+#include "filters.h"
 
 Throttle::Throttle(PinName pin, float min, float max) {
     _in = new AnalogVoltageSensor(pin, 1.0f);