Revision 13:41d102a53caf, committed 2016-10-30

Comitter:

dicarloj

Date:

Sun Oct 30 02:06:03 2016 +0000

Parent:

12:5723a4fa5864

Commit message:

IT WORKS THIS VERSION

Changed in this revision

diff -r 5723a4fa5864 -r 41d102a53caf config.h
--- a/config.h	Sun Oct 30 01:37:30 2016 +0000
+++ b/config.h	Sun Oct 30 02:06:03 2016 +0000
@@ -37,4 +37,6 @@
 #define set_dtc(phase, value) *phase = (value)
 #define PI 3.141593f
 
+#define Q_MAX 50
+
 #endif
\ No newline at end of file

diff -r 5723a4fa5864 -r 41d102a53caf main.cpp
--- a/main.cpp	Sun Oct 30 01:37:30 2016 +0000
+++ b/main.cpp	Sun Oct 30 02:06:03 2016 +0000
@@ -4,13 +4,15 @@
 #include "FastPWM.h"
 #include "Transforms.h"
 #include "config.h"
+#include "pwm_in.h"
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 DigitalOut toggle(PC_10);
-
+PWM_IN p_in(PB_8, 1100, 1900);
+bool control_enabled = false;
 PositionSensorEncoder pos(CPR, 0);
 
 Serial pc(USBTX, USBRX);
@@ -25,11 +27,38 @@
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = 0.0f, q_ref = 50.0f;
 
+
 void commutate();
 void zero_current();
 void config_globals();
 void startup_msg();
 
+void go_enabled()
+{
+    d_integral = 0.0f;
+    q_integral = 0.0f;
+    control_enabled = true;
+    en = 1;
+}
+
+void go_disabled()
+{
+    control_enabled = false;
+    en = 0;
+}
+
+float fminf(float a, float b)
+{
+    if(a < b) return a;
+    return b;
+}
+
+float fmaxf(float a, float b)
+{
+    if(a > b) return a;
+    return b;
+}
+
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
         toggle = 1;
@@ -144,6 +173,9 @@
 }    
 
 void commutate() {
+    if(control_enabled && !p_in.get_enabled()) go_disabled();
+    if(!control_enabled && p_in.get_enabled()) go_enabled();
+    q_ref = p_in.get_throttle() * Q_MAX;
     p = pos.GetElecPosition() - POS_OFFSET;
     if (p < 0) p += 2 * PI;
     
@@ -177,8 +209,16 @@
     if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
     if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX;
     
-    vd = KP * d_err + d_integral;
-    vq = KP * q_err + q_integral;
+    if(control_enabled)
+    {
+        vd = KP * d_err + d_integral;
+        vq = KP * q_err + q_integral;
+    }
+    else
+    {
+        vd = 0;
+        vq = 0;
+    }
     
     if (vd < -1.0f) vd = -1.0f;
     if (vd > 1.0f) vd = 1.0f;
@@ -198,6 +238,11 @@
     float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
     float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
     
+    float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
+    va = va - voff;
+    vb = vb - voff;
+    vc = vc - voff;
+    
     set_dtc(a, 0.5f + 0.5f * va);
     set_dtc(b, 0.5f + 0.5f * vb);
     set_dtc(c, 0.5f + 0.5f * vc);

diff -r 5723a4fa5864 -r 41d102a53caf pwm_in.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pwm_in.cpp	Sun Oct 30 02:06:03 2016 +0000
@@ -0,0 +1,56 @@
+#include "pwm_in.h"
+#include "mbed.h"
+
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+float constrain(float in, float min, float max)
+{
+    if(in > max) return max;
+    if(in < min) return min;
+    return in;
+}
+
+PWM_IN::PWM_IN(PinName pin, int usec_min, int usec_max)
+{
+    int_in = new InterruptIn(pin);
+    dig_in = new DigitalIn(pin);
+    int_in->rise(this, &PWM_IN::handle_rise);
+    int_in->fall(this, &PWM_IN::handle_fall);
+    this->usec_min = usec_min;
+    this->usec_max = usec_max;
+}
+
+
+bool PWM_IN::get_enabled()
+{
+    return enabled;
+}
+
+void PWM_IN::handle_rise()
+{
+    enabled = true;
+    timer.stop();
+    timer.reset();
+    timer.start();
+    was_on = true;
+}
+
+void PWM_IN::handle_fall()
+{
+    was_on = false;
+    usecs = timer.read_us();
+    timer.stop();
+    timer.reset();
+    timer.start();
+}
+
+float PWM_IN::get_throttle()
+{
+    if(timer.read_us() > 40000) enabled = false;
+    if(!enabled) return -1;
+    return constrain(map((float)usecs, usec_min, usec_max, 0, 1), 0, 1);
+}
+

diff -r 5723a4fa5864 -r 41d102a53caf pwm_in.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pwm_in.h	Sun Oct 30 02:06:03 2016 +0000
@@ -0,0 +1,25 @@
+#ifndef _pwm_in
+#define _pwm_in
+#include "mbed.h"
+
+class PWM_IN
+{
+    public:
+    PWM_IN(PinName pin, int usec_min, int usec_max);
+    bool get_enabled();
+    float get_throttle();
+    
+    
+    private:
+    InterruptIn* int_in;
+    DigitalIn*   dig_in;
+    Timer timer;
+    bool was_on;
+    bool enabled;
+    void handle_rise();
+    void handle_fall();
+    int usecs;
+    int usec_min, usec_max;
+    
+};
+#endif
\ No newline at end of file