// Calculate the running times used by this segment. The running times // define the velocity profile of the segment. They are calculated when // starting and ending states of the segment have been found. virtual bool Calculate( void ) { if( calculated ) return true; calculated = true; // If not using jerk limits, use simpler calculations if( !usingJerkLimits() ) { CalcNoJrk(); return false; } // Calculate the sub-segment times CalcTimes(); // Fill in the position, velocity and accel for // the end of each of the sub-segments. double p = 0; double v = getVelStart(); double a = 0; double Jmax = getMaxJrk(); for( int i=0; i<7; i++ ) { double J = Jmax * jerkMult[i]; double t = SegT[i]; p += v*t + a*t*t/2 + J*t*t*t/6; v += a*t + J*t*t/2; a += J*t; SegP[i] = p; SegV[i] = v; SegA[i] = a; } return false; }
void CalcRefVec30() { int16_t dPWM1,dPWM2,dPWM3; if( SCP.V1_30 >= 0 ) { // (xx1) if( SCP.V2_30 >= 0 ) { // (x11) // Must be Sector 3 since Sector 7 not allowed // Sector 3: (0,1,1) 0-60 degrees SCP.T1 = SCP.V2_30; // decrease SCP.T2 = SCP.V1_30; // increase CalcTimes(); dPWM1 = SCP.Ts; // changed dPWM2 = SCP.Tm; dPWM3 = SCP.Tl; SCP.sector =0; } else { // (x01) if( SCP.V3_30 >= 0 ) { // Sector 5: (1,0,1) 120-180 degrees SCP.T1 = SCP.V1_30; // decrease SCP.T2 = SCP.V3_30; // increase CalcTimes(); dPWM1 = SCP.Tl; dPWM2 = SCP.Ts; dPWM3 = SCP.Tm; SCP.sector =120; } else { // Sector 1: (0,0,1) 60-120 degrees SCP.T1 = -SCP.V2_30; SCP.T2 = -SCP.V3_30; CalcTimes(); dPWM1 = SCP.Tm; dPWM2 = SCP.Ts; dPWM3 = SCP.Tl; SCP.sector =60; } } } else { // (xx0) if( SCP.V2_30 >= 0 ) { // (x10) if( SCP.V3_30 >= 0 ) { // Sector 6: (1,1,0) 240-300 degrees SCP.T1 = SCP.V3_30; SCP.T2 = SCP.V2_30; CalcTimes(); dPWM1 = SCP.Tm; dPWM2 = SCP.Tl; dPWM3 = SCP.Ts; SCP.sector =240; } else { // Sector 2: (0,1,0) 300-0 degrees SCP.T1 = -SCP.V3_30; SCP.T2 = -SCP.V1_30; CalcTimes(); dPWM1 = SCP.Ts; dPWM2 = SCP.Tl; dPWM3 = SCP.Tm; SCP.sector =300; } } else { // (x00) // Must be Sector 4 since Sector 0 not allowed // Sector 4: (1,0,0) 180-240 degrees SCP.T1 = -SCP.V1_30; SCP.T2 = -SCP.V2_30; CalcTimes(); dPWM1 = SCP.Tl; dPWM2 = SCP.Tm; dPWM3 = SCP.Ts; SCP.sector =180; } } // achtung SCP.P1=dPWM1; SCP.P2=dPWM2; SCP.P3=dPWM3; P1DC1 = dPWM1; //1023; // 10 bits resolution P1DC2 = dPWM2; // 10 bits resolution P1DC3 = dPWM3; //printf(" PWM: %d %d %d S:%d",dPWM1,dPWM2,dPWM3, SCP.sector ); }