Bayley Wang
/
qonly_controller
derp
Diff: main.cpp
- Revision:
- 14:10743610ecb5
- Parent:
- 11:825203ff4371
--- a/main.cpp Mon Apr 18 20:08:05 2016 +0000 +++ b/main.cpp Mon Apr 18 22:53:35 2016 +0000 @@ -26,17 +26,13 @@ #define K_LOOP 0.02 #define KI_BASE 0.008 -#define BUS_VOLTAGE 20.0 +#define BUS_VOLTAGE 200.0 #define KP (K_LOOP / BUS_VOLTAGE) #define KI (KI_BASE * K_LOOP / BUS_VOLTAGE) #define INTEGRAL_MAX 1.0f -#define THROTTLE_MAX 2000.0f -#define THROTTLE_ERROR 5000.0f -#define THROTTLE_MIN 1500.0f - #define Q_REF_MAX (-200.0) FastPWM *a; @@ -44,7 +40,7 @@ FastPWM *c; DigitalOut en(EN); -InterruptIn throttle_interrupt(PB_8); +DigitalIn throttle_in(PB_8); PositionSensorEncoder pos(CPR, 0); int adval1, adval2; @@ -53,8 +49,6 @@ float throttle = 0.0f; int edge_seen = 0, watchdog_divider = 0; - -int err_throttle_too_low = 0, err_throttle_too_high = 0; extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF) { @@ -85,31 +79,18 @@ q = -alpha * sin_p - beta * cos_p; - //if (throttle < THROTTLE_MIN || throttle > THROTTLE_ERROR) throttle = THROTTLE_MIN; - if (throttle < THROTTLE_MIN) { - throttle = THROTTLE_MIN; - err_throttle_too_low = 1; - } - - /* - if(edge_seen == 0) { - throttle = THROTTLE_MIN; + float throttle_scaler; + + if (throttle_in.read() == 0) { + throttle_scaler = 0.0f; en = 0; } else { + throttle_scaler = 1.0f; en = 1; } - */ - - if (throttle > THROTTLE_ERROR) { - throttle = THROTTLE_MIN; - err_throttle_too_high = 1; - } - - if (throttle > THROTTLE_MAX) throttle = THROTTLE_MAX; - float throttle_scaler = (throttle - THROTTLE_MIN) / (THROTTLE_MAX - THROTTLE_MIN); double q_err = Q_REF_MAX * (double) throttle_scaler - q; - + //DAC->DHR12R2 = (unsigned int) (q_err * 20 + 2048); q_integral += q_err * KI; @@ -119,39 +100,16 @@ vq = KP * q_err + q_integral; if (vq < -1.0f) vq = -1.0f; if (vq > 1.0f) vq = 1.0f; - + //DAC->DHR12R2 = (unsigned int) (vq * 2000 + 2048); - DAC->DHR12R2 = (unsigned int) (throttle_scaler * 2000 + 2048); - - if (err_throttle_too_low || err_throttle_too_high || !edge_seen) { - en = 0; - } else { - en = 1; - } - + *a = 0.5f + 0.5f * vq * sinf(p); *b = 0.5f + 0.5f * vq * sinf(p + 2 * PI / 3); *c = 0.5f + 0.5f * vq * sinf(p - 2 * PI / 3); - - watchdog_divider++; - if (watchdog_divider == 15000) { - watchdog_divider = 0; - edge_seen = 0; - } } TIM1->SR = 0x00; } -void throttle_rise() { - TIM5->CNT = 0; - edge_seen = 1; -} - -void throttle_fall() { - throttle = TIM5->CNT; - edge_seen = 1; -} - int main() { //Enable clocks for GPIOs RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; @@ -224,8 +182,7 @@ en = 1; - throttle_interrupt.rise(throttle_rise); - throttle_interrupt.fall(throttle_fall); + throttle_in.mode(PullUp); for (;;) { }