# Constants
hz2rps = 2*np.pi
rps2hz = 1/hz2rps
#%% Define the frequency selection and distribution of the frequencies into the signals
freqRate_hz = 50;
freqInit_rps = 0.1 * hz2rps
freqFinal_rps = 10 * hz2rps
timeDur_s = 10.0
ampInit = 1.0
ampFinal = 1.0
time_s = np.linspace(0, timeDur_s, int(timeDur_s * freqRate_hz) + 1)
sig, ampChirp, freqChirp_rps = GenExcite.Chirp(freqInit_rps, freqFinal_rps, time_s, ampInit, ampFinal, freqType = 'linear', ampType = 'linear', initZero = 1)
## Results
plt.figure()
plt.subplot(3,1,1)
plt.plot(time_s, sig); plt.grid()
plt.ylabel('Signal (nd)');
plt.subplot(3,1,2)
plt.plot(time_s, ampChirp); plt.grid()
plt.ylabel('ampitude (nd)');
plt.subplot(3,1,3)
plt.plot(time_s, freqChirp_rps * rps2hz); plt.grid()
plt.xlabel('Time (s)'); plt.ylabel('Frequency (Hz)')
plt.show()
freqMinDes_rps = (1/timeDur_s) * hz2rps * np.ones(numChan)
freqMaxDes_rps = 15 * hz2rps * np.ones(numChan)
freqStepDes_rps = (10 / 50) * hz2rps
methodSW = 'zip' # "zippered" component distribution
## Generate MultiSine Frequencies
freqExc_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW)
timeDur_s = time_s[-1] - time_s[0]
## Generate Schroeder MultiSine Signal
ampElem_nd = np.ones_like(freqExc_rps) ## Approximate relative signal amplitude, create flat
sigList, phaseElem_rad, sigElem = GenExcite.MultiSine(freqExc_rps, ampElem_nd, sigIndx, time_s, costType = 'Schroeder', phaseInit_rad = 0, boundPhase = True, initZero = True, normalize = 'rms');
sigPeakFactor = GenExcite.PeakFactor(sigList)
if 0:
sigList[0], _, _ = GenExcite.Chirp(freqMinDes_rps, freqMaxDes_rps, time_s)
#%%
# ampList = np.array([3, 5, 10, 20])
ampList = np.arange(1.0, 40.1, 0.5)
# ampList = np.array([1, 1.5, 2.0, 2.5, 20, 21, 22, 23, 24, 25])
# Create Servo Object (HiTec HS-225BB)
freqNat_hz = 6.0
freqNat_rps = freqNat_hz * hz2rps
objServo = Servo.Servo(1/freqRate_hz, freqNat_rps = freqNat_rps, damp = 0.8)
objServo.freeplay = 1.0 # @ 2.0
objServo.timeDelay_s = 50 / 1000 # this ends up rounded to an integer (timeDelay_s * freqRate_hz)
# objServo.cmdLim = 20
objServo.pLim = 20