101 lines
3.8 KiB
C
101 lines
3.8 KiB
C
//**************************************************************
|
|
void trapBuildSymAcc(float Disp_rad, float Vplat, float Acc)
|
|
{
|
|
float tAcc = Vplat / Acc;
|
|
float dAcc = Vplat * tAcc;
|
|
float dPlat = Disp_rad - dAcc;
|
|
float tPlat = dPlat / Vplat;
|
|
|
|
trapAccT = Acc; // acceleration rate
|
|
accT = int(round(tAcc / TSAMP)); // time duration of acceleration
|
|
platT = int(round(tPlat / TSAMP)); // time duration of constant speed
|
|
float dTrap = (accT + platT) * TSAMP * Vplat; // total displacement during trapezoid profile
|
|
|
|
Serial.println();
|
|
Serial.print("tAcc = "); Serial.println(tAcc);
|
|
Serial.print("dAcc = "); Serial.println(dAcc);
|
|
Serial.print("dPlat = "); Serial.println(dPlat);
|
|
Serial.print("tPlat = "); Serial.println(tPlat);
|
|
Serial.print("accT = "); Serial.println(accT);
|
|
Serial.print("platT = "); Serial.println(platT);
|
|
Serial.print("dTrap = "); Serial.println(dTrap);
|
|
}
|
|
|
|
//*********************************************************************
|
|
void trapRefVel(float specDisp_rad, float specVel, float specAcc, int Ncycles) // trap velocity profile
|
|
{
|
|
if (!oldProfileEn && isProfileEn)
|
|
{
|
|
//specDisp_rad = 300.0; specVel = 150.0; specAcc = 800.0;
|
|
trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
dwellStartT = 1; dwellEndT = 1; specVdist = 0.0;
|
|
|
|
//specDisp_rad = 300.0, specVel = 100.0, specAcc = 200.0;
|
|
//trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
//dwellStartT = 50, dwellEndT = 200; specVdist = 2.0;
|
|
|
|
//specDisp_rad = 206.9, specVel = 110.0, specAcc = 180.0;
|
|
//trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
//dwellGoT = 300, dwellUpT = 500, dwellEndT = 200; specVdist = abs(V_DIST);
|
|
|
|
// Times when speed changes
|
|
T0 = dwellStartT;
|
|
T1 = T0 + accT; T2 = T1 + platT; T3 = T2 + accT; T4 = T3 + dwellEndT;
|
|
T5 = T4 + accT; T6 = T5 + platT; T7 = T6 + accT; T8 = T7 + dwellEndT;
|
|
|
|
//T0 = dwellGoT;
|
|
//T1 = T0 + accT, T2 = T1 + platT, T3 = T2 + accT, T4 = T3 + dwellUpT;
|
|
//T5 = T4 + accT, T6 = T5 + platT, T7 = T6 + accT, T8 = T7 + dwellEndT;
|
|
|
|
// Times when disturbances are applied and values
|
|
D1 = T3 + 100; D2 = D1 + 200;
|
|
dD1 = specVdist; dD2 = -specVdist;
|
|
|
|
//simJumpOn_simJumpOff();
|
|
//simJumpOn_simFileOff();
|
|
//weightOn_simJumpOff();
|
|
//weightOn_simFileOff();
|
|
}
|
|
tick = timeElapsedTicks % T8; //modulo allows rerunning of profile. Remove for single run.
|
|
|
|
// ---- Command profile -------------------------------------
|
|
oldProfileEn = isProfileEn;
|
|
if (tick < T0) refAccT = 0.0;
|
|
else if (tick < T1) refAccT = dir * trapAccT;
|
|
else if (tick < T2) refAccT = 0.0;
|
|
else if (tick < T3) refAccT = -dir * trapAccT;
|
|
else if (tick < T4) refAccT = 0.0;
|
|
//else if (tick == T4) isProfileEn = false;
|
|
|
|
else if (tick < T5) refAccT = -dir * trapAccT;
|
|
else if (tick < T6) refAccT = 0.0;
|
|
else if (tick < T7) refAccT = dir * trapAccT;
|
|
else if (tick < T8) refAccT = 0.0;
|
|
else if (tick == T8) isProfileEn = false;
|
|
|
|
//---- Disturbance profile -----------------------------------
|
|
if (tick < D1) Vdist = 0.0;
|
|
else if (tick == D1) Vdist += dD1 * specVdist; // module, sim load
|
|
else if (tick == D2) Vdist += dD2 * specVdist;
|
|
else if (tick == D3) Vdist += dD3 * specVdist;
|
|
else if (tick == D4) Vdist += dD4 * specVdist;
|
|
else if (tick == D5) Vdist += dD5 * specVdist;
|
|
else if (tick == D6) Vdist += dD6 * specVdist;
|
|
else if (tick == D7) Vdist += dD7 * specVdist;
|
|
else if (tick == D8) Vdist += dD8 * specVdist;
|
|
|
|
//---- Profile integration -----------------------------------
|
|
if (isProfileEn) {
|
|
refVelT += refAccT * TSAMP;
|
|
refPosT += refVelT * TSAMP;
|
|
}
|
|
else {
|
|
refAccT = 0.0;
|
|
refVelT = 0.0;
|
|
}
|
|
|
|
isShowStats = (oldProfileEn && !isProfileEn);
|
|
if ((timeElapsedTicks/T8)>=Ncycles) {isProfileEn=false; isModuleEn=false;}; // stop after 4 profiles
|
|
|
|
}
|
|
|