Bayley Wang
/
qonly_controller
derp
Diff: main.cpp
- Revision:
- 11:825203ff4371
- Parent:
- 10:7624146c5945
- Child:
- 12:0811d08424e1
- Child:
- 14:10743610ecb5
diff -r 7624146c5945 -r 825203ff4371 main.cpp --- a/main.cpp Tue Apr 12 13:09:36 2016 +0000 +++ b/main.cpp Mon Apr 18 20:08:05 2016 +0000 @@ -33,18 +33,18 @@ #define INTEGRAL_MAX 1.0f -#define THROTTLE_MAX 2400.0f -#define THROTTLE_ERROR 4800.0f -#define THROTTLE_MIN 500.0f +#define THROTTLE_MAX 2000.0f +#define THROTTLE_ERROR 5000.0f +#define THROTTLE_MIN 1500.0f -#define Q_REF_MAX 20.0 +#define Q_REF_MAX (-200.0) FastPWM *a; FastPWM *b; FastPWM *c; DigitalOut en(EN); -InterruptIn throttle_interrupt(PC_10); +InterruptIn throttle_interrupt(PB_8); PositionSensorEncoder pos(CPR, 0); int adval1, adval2; @@ -53,6 +53,8 @@ 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) { @@ -83,13 +85,31 @@ q = -alpha * sin_p - beta * cos_p; - if(edge_seen == 0) throttle = THROTTLE_MIN; - if (throttle < THROTTLE_MIN || throttle > THROTTLE_ERROR) throttle = THROTTLE_MIN; + //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; + en = 0; + } else { + 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; @@ -99,9 +119,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);