N K
/
analoghalls_part_4
yup
Fork of analoghalls by
isr.cpp
- Committer:
- nki
- Date:
- 2015-02-26
- Revision:
- 5:eeb8af99cb6c
- Parent:
- 4:f18f6bc5e1fd
- Child:
- 6:4960629abb90
File content as of revision 5:eeb8af99cb6c:
#include "isr.h" #include "mbed.h" #include "shared.h" #include "constants.h" #include "util.h" #include "math.h" void dtc_update() { #ifndef __SVM float ia = motor->angle - motor->sensor_phase; if (ia < 0) ia += 360.0f; if (ia >= 360.0f) ia -= 360.0f; float ib = ia - 120.0f; if (ib < 0) ib += 360.0f; 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; #else float ix = 120.0f - motor->angle; if (ix < 0) ix += 360.0f; if (ix >= 360.0f) ix -= 360.0f; float dx = sinetab[(int) ix]; float iy = motor->angle; if (iy < 0) iy += 360.0f; if (iy >= 360.0f) iy -= 360.0f; float dy = sinetab[(int) iy]; int sector = (int) (iy / 60.0f) + 1; switch(sector) { case 1: motor->dtc_a = 1.0f - dx - dy; motor->dtc_b = 1.0f + dx - dy; motor->dtc_c = 1.0f + dx + dy; break; case 2: motor->dtc_a = 1.0f - dx + dy; motor->dtc_b = 1.0f - dx - dy; motor->dtc_c = 1.0f + dx + dy; break; case 3: motor->dtc_a = 1.0f + dx + dy; motor->dtc_b = 1.0f - dx - dy; motor->dtc_c = 1.0f + dx - dy; break; case 4: motor->dtc_a = 1.0f + dx + dy; motor->dtc_b = 1.0f - dx + dy; motor->dtc_c = 1.0f - dx + dy; //suspect break; case 5: motor->dtc_a = 1.0f + dx - dy; motor->dtc_b = 1.0f + dx + dy; motor->dtc_c = 1.0f - dx - dy; break; case 6: motor->dtc_a = 1.0f - dx - dy; motor->dtc_b = 1.0f + dx + dy; motor->dtc_c = 1.0f - dx + dy; break; default: break; } #endif if (motor->halt || motor->debug_stop) { setDtcA(0); setDtcB(1.0f); setDtcC(0); } else { setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET); setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET); setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET); } } void pos_update() { float ascaled = 2*(((float)analoga-0.256f)/(0.484f-0.256f)-0.5f); float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-0.5f); float x = bscaled / ascaled; unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE; if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1; if(ascaled<0){ if(bscaled<0) motor->angle = 90 - arctan[index]; if(bscaled>0) motor->angle = 90 + arctan[index]; } if(ascaled>0){ if(bscaled>0) motor->angle = 270 - arctan[index]; if(bscaled<0) motor->angle = 270 + arctan[index]; } if(motor->angle > 360.0f) motor->angle = 360.0f; if(motor->angle < 0) motor->angle = 0; //calculate rotational rate float speed_raw = motor->angle - motor->lastangle; //these IFs check for theta = 0 crossings if(speed_raw < -180.0f){ speed_raw = motor->angle - motor->lastangle + 360.0f; } if(speed_raw > 180.0f){ speed_raw = motor->angle - motor->lastangle - 360.0f; } motor->lastangle = motor->angle; motor->speed = (1.0f - SPEED_LPF) * speed_raw + SPEED_LPF * motor->speed; #ifdef __DEBUG skipidx++; if (skipidx == SKIP) { skipidx = 0; if (!motor->halt) { fbuffer[bufidx] = motor->angle; bufidx++; } } if (bufidx == DBG_BUF_SZ) { motor->debug_stop = 1; } #endif } void throttle_update() { float throttle_raw = throttle_read; 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; if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) { motor->halt = 1; } else { motor->halt = 0; } } 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 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 }