// 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 );
}
