![](/media/cache/profiles/916954106a13204fa4909ae2d48e0602.jpg.50x50_q85.jpg)
working version
Fork of foc-ed_in_the_bot_compact by
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
--- 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
--- 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);
--- /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); +} +
--- /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