N K
/
analoghalls_part_4
yup
Fork of analoghalls by
Diff: isr.cpp
- Revision:
- 6:4960629abb90
- Parent:
- 5:eeb8af99cb6c
--- a/isr.cpp Thu Feb 26 09:50:36 2015 +0000 +++ b/isr.cpp Thu Feb 26 14:09:19 2015 +0000 @@ -15,9 +15,9 @@ float ic = ia + 120.0f; if (ic >= 360.0f) ic -= 360.0f; - motor->dtc_a = sinetab[(int) (ia)] * motor->throttle; - motor->dtc_b = sinetab[(int) (ib)] * motor->throttle; - motor->dtc_c = sinetab[(int) (ic)] * motor->throttle; + motor->dtc_a = sinetab[(int) (ia)] * motor->dtc; + motor->dtc_b = sinetab[(int) (ib)] * motor->dtc; + motor->dtc_c = sinetab[(int) (ic)] * motor->dtc; #else float ix = 120.0f - motor->angle; if (ix < 0) ix += 360.0f; @@ -132,6 +132,7 @@ throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB); if (throttle_raw < 0.0f) throttle_raw = 0.0f; motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle; + motor->command = motor->throttle; if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) { motor->halt = 1; } else { @@ -140,17 +141,37 @@ } void isense_update() { + float ia = ia_read; float ib = ib_read; - float irms = sqrt(ia*ia + ib*ib + (ia+ib)*(ia+ib)); + + if(motor->halt == 1){ //zero the current sensors + motor->iazero = ia; + motor->ibzero = ib; + } + + ia = ia-motor->iazero; + ib = ib-motor->ibzero; + + float irms = ia*ia + ib*ib + (ia+ib)*(ia+ib); + + motor->current = (1.0f - CURRENT_LPF) * irms + CURRENT_LPF * motor->current; + + float iset = IMAX * motor->command; + motor->iP = iset - motor->current; +// motor->iI = motor->iIlast + motor->iP; +// motor->iD = motor->iP - motor->ilast; + motor->dtc = Kp_i*motor->iP; //+ Ki_i*motor->iI + Kd_i*motor->iD; + + if(motor->dtc < 0.0f){ + motor->dtc = 0.0f; + } + if(motor->dtc > 1.0f){ + motor->dtc = 1.0f; + } + - if(motor->halt == 1){ //zero the current sensor OMG THIS IS SO SCARY OMG - motor->izero = irms; - } - motor->current = irms-motor->izero; - - //float iset = motor->imax * motor->throttle //float error = irms - iset } \ No newline at end of file