import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import msdParam as P from signalGenerator import signalGenerator from msdAnimation import msdAnimation from msdDynamics import msdDynamics from plotData import plotData # instantiate reference input classes msd = msdDynamics() reference = signalGenerator(amplitude=0.01, frequency=0.02) fRef = signalGenerator(amplitude=5, frequency=.5) zRef = signalGenerator(amplitude=2, frequency=0.1) # instantiate the simulation plots and animation dataPlot = plotData() animation = msdAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop r = reference.square(t) # set variables # Force = [0] t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate Force = fRef.sin(t) msd.propagateDynamics(Force) # Propagate the dynamics
sys.path.append('..') # add parent directory import matplotlib.pyplot as plt import numpy as np import D_param as P from D_dynamics import D_dynamics from D_controller import D_controller from signalGenerator import signalGenerator from D_animation import D_animation from plotData import plotData # from preFilter import LSFControl as LSF # instantiate arm, controller, and reference classes msd = D_dynamics() ctrl = D_controller() # F = LSF() reference = signalGenerator(amplitude=1.0, frequency=0.05, y_offset=0.0) # instantiate the simulation plots and animation dataPlot = plotData() animation = D_animation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate u = ctrl.u(ref_input, msd.outputs()) # Calculate the control value msd.propagateDynamics(u) # Propagate the dynamics
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import bobParam as P from signalGenerator import signalGenerator from bobAnimation import bobAnimation from plotData import plotData from bobDynamics import bobDynamics # instantiate reference input classes bob = bobDynamics() reference = signalGenerator(amplitude=0.5, frequency=0.1) zRef = signalGenerator(amplitude=3, frequency=0.1) thetaRef = signalGenerator(amplitude=1/4*np.pi, frequency=0.1) fRef = signalGenerator(amplitude=3, frequency=0.05) # instantiate the simulation plots and animation dataPlot = plotData() animation = bobAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate F = fRef.sin(t) # F = [0] bob.propagateDynamics(F) # Propagate the dynamics t = t + P.Ts # advance time by Ts
import matplotlib.pyplot as plt import numpy as np import rodMassParam as P from signalGenerator import signalGenerator from rodMassAnimation import rodMassAnimation from dataPlotter import dataPlotter from rodMassDynamics import rodMassDynamics from controllerLoop import controllerLoop # instantiate system, controller, and reference classes rodMass = rodMassDynamics() controller = controllerLoop() reference = signalGenerator(amplitude=20*np.pi/180.0, frequency=0.1) disturbance = signalGenerator(amplitude=0.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = rodMassAnimation() t = P.t_start y = rodMass.h() while t < P.t_end: t_next_plot = t + P.t_plot while t < t_next_plot: r = reference.square(t) d = disturbance.step(t) n = 0.0 #noise.random(t) u = controller.update(r, y + n) y = rodMass.update(u + d) t = t + P.Ts # update animation and data plots
sys.path.append('..') # add parent directory import matplotlib.pyplot as plt import numpy as np import armParam as P from armDynamics import armDynamics from armController import armController from signalGenerator import signalGenerator from armAnimation import armAnimation from plotData import plotData from plotObserverData import plotObserverData # instantiate arm, controller, and reference classes arm = armDynamics() ctrl = armController() reference = signalGenerator(amplitude=30 * np.pi / 180.0, frequency=0.05) # instantiate the simulation plots and animation dataPlot = plotData() animation = armAnimation() observerPlot = plotObserverData() # set disturbance input disturbance = 0.01 t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot
import matplotlib.pyplot as plt import sys sys.path.append('..') # add parent directory import ballbeamParam as P from signalGenerator import signalGenerator from ballbeamAnimation import ballbeamAnimation from plotData import plotData from ballbeamDynamics import ballbeamDynamics from ballbeamTopAnimation import ballbeamTopAnimation # instantiate ballbeam, controller, and reference classes ballbeam = ballbeamDynamics() x_reference = signalGenerator(amplitude=0.5, frequency=0.02) y_reference = signalGenerator(amplitude=0.5, frequency=0.02) theta = signalGenerator(amplitude=.1, frequency=1) thdot = 0. phi = signalGenerator(amplitude=.1, frequency=1) phdot = 0. # instantiate the simulation plots and animation dataPlot = plotData() x_animation = ballbeamAnimation(title="x axis") y_animation = ballbeamAnimation(title="y axis") top_animation = ballbeamTopAnimation(title="top view") t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = [x_reference.square(t),y_reference.square(t)] # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import ballbeamParam as P from signalGenerator import signalGenerator from ballbeamAnimation import ballbeamAnimation from dataPlotter import dataPlotter # instantiate reference input classes reference = signalGenerator(amplitude=1.0, frequency=0.05, y_offset=0.1) zRef = signalGenerator(amplitude=0.25, frequency=0.05, y_offset=0.25) thetaRef = signalGenerator(amplitude=.1 * np.pi, frequency=.025) fRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = ballbeamAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables r = reference.sin(t) z = zRef.sin(t) theta = thetaRef.sin(t) f = fRef.sin(t) # update animation state = np.array([[z], [theta], [0.0], [0.0]]) animation.update(state) dataPlot.update(t, r, state, f)
import matplotlib.pyplot as plt import sys sys.path.append('..') # add parent directory import armParam as P from signalGenerator import signalGenerator from armAnimation import armAnimation from plotData import plotData from armDynamics import armDynamics # instantiate arm, controller, and reference classes arm = armDynamics() reference = signalGenerator(amplitude=0.01, frequency=0.02) torque = signalGenerator(amplitude=0.2, frequency=0.05) # instantiate the simulation plots and animation dataPlot = plotData() animation = armAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate tau = torque.square(t) arm.propagateDynamics(tau) # Propagate the dynamics t = t + P.Ts # advance time by Ts # update animation and data plots animation.drawArm(arm.states()) dataPlot.updatePlots(t, ref_input, arm.states(), tau)
from dataPlotter import dataPlotter from vtolDynamics import vtolDynamics import vtolController as PD import numpy as np from dataPlotterObserver import dataPlotterObserver # Instantiate VTOL modules vtol = vtolDynamics(alpha=0.0) #lon control input ###########lon_controller_f8 = PD.PDController(P.kp_lon8,P.kd_lon8) lon_controller = PD.PDController() #lat control input ###########lat_controller = PD.PDController(kp2=P.kp_lat_theta8,kd2 = P.kd_lat_theta8,kp = P.kp_lat_z8,kd = P.kd_lat_z8) lat_controller = PD.PDController() reference = signalGenerator(amplitude=0.5, frequency=0.05) disturbance = signalGenerator(amplitude=0.5) noise_z = signalGenerator(amplitude=0.01) noise_th = signalGenerator(amplitude=0.01) noise_h = signalGenerator(amplitude=0.01) #reference h_reference = signalGenerator(amplitude=1, frequency=0.05, y_offset=2) theta_reference = signalGenerator(amplitude=0, frequency=0.05, y_offset=0) z_reference = signalGenerator(amplitude=2.5, frequency=0.05, y_offset=3) disturbance = signalGenerator(amplitude=0.1) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = vtolAnimation() dataPlotObserver = dataPlotterObserver()
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import ballbeamParam as P from signalGenerator import signalGenerator from ballbeamAnimation import ballbeamAnimation from ballbeamTopAnimation import ballbeamTopAnimation from plotData import plotData # instantiate reference input classes x_reference = signalGenerator(amplitude=0.5, frequency=0.1) y_reference = signalGenerator(amplitude=0.5, frequency=0.1) xRef = signalGenerator(amplitude=0.5, frequency=0.1) yRef = signalGenerator(amplitude=0.5, frequency=0.1) thetaRef = signalGenerator(amplitude=2.0 * np.pi / 4.0, frequency=0.1) # angle about y causes x motion phiRef = signalGenerator(amplitude=2.0 * np.pi / 4.0, frequency=0.1) # angle about x causes y motion # instantiate the simulation plots and animation dataPlot = plotData() x_animation = ballbeamAnimation(title="x axis") y_animation = ballbeamAnimation(title="y axis") top_animation = ballbeamTopAnimation(title="top view") t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables
Camera = True if Camera: from BallPosition import BallPosition BP = BallPosition(2) # center = BP.Position("green") xy = np.asarray(BP.Position("orange")) ctrl = ballbeamController(xy[0], xy[1]) else: ctrl = ballbeamController(P.x0, P.y0) # ctrl = ballbeamController() # instantiate ballbeam, controller, and reference classes ballbeam = ballbeamDynamics() if Camera: x_reference = signalGenerator(amplitude=0.0, frequency=0.051) y_reference = signalGenerator(amplitude=0.0, frequency=0.051)#, t_offset=True) # x_reference = signalGenerator(amplitude=0.0325, frequency=0.051) # y_reference = signalGenerator(amplitude=0.0325, frequency=0.051)#, t_offset=True) else: x_reference = signalGenerator(amplitude=0.0, frequency=0.051) y_reference = signalGenerator(amplitude=0.0, frequency=0.051)#, t_offset=True) # x_reference = signalGenerator(amplitude=0.0325, frequency=0.051) # y_reference = signalGenerator(amplitude=0.0325, frequency=0.051)#, t_offset=True) # instantiate the simulation plots and animation dataPlot = plotData() # TopData = plotTopData() top_animation = ballbeamTopAnimation(title="top view") t = P.t_start # time starts at t_start
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import massParam as P from signalGenerator import signalGenerator from massAnimation import massAnimation from dataPlotter import dataPlotter # instantiate reference input classes reference = signalGenerator(amplitude=1.0, frequency=0.2, y_offset=0.1) thetaRef = signalGenerator(amplitude=2.0 * np.pi, frequency=0.1) tauRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = massAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables z = reference.sin(t) ctrl = 0.0 # update animation state = np.array([[z], [0.0]]) animation.update(state) dataPlot.update(t, z, state, ctrl) #plt.show()
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import armParam as P from signalGenerator import signalGenerator from armAnimation import armAnimation #from plotData import plotData from slider_input import * # instantiate reference input classes reference = signalGenerator(amplitude=0.5, frequency=0.1) #thetaRef = signalGenerator(amplitude=0.2*np.pi, frequency=0.1) tauRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation #dataPlot = plotData() animation = armAnimation() t = P.t_start # time starts at t_start obj = mySlider() while t < P.t_end: # main simulation loop # set variables r = reference.square(t) #theta = thetaRef.sin(t) theta = obj.getValue() print(theta) obj.update(theta) tau = tauRef.sawtooth(t) # update animation state = [theta, 0.0]
import sys sys.path.append('..') # add parent directory import matplotlib.pyplot as plt import numpy as np import Param as P from springDynamics1 import Dynamics from springController import springController from signalGenerator import signalGenerator from Animation import Animation from dataPlotter import dataPlotter # instantiate arm, controller, and reference classes spring = Dynamics(0.2) controller = springController() reference = signalGenerator( amplitude=1, frequency=0.05 ) #amplitude chosen according to 1 m step input, frequency = wn disturbance = signalGenerator(amplitude=0.0) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = Animation() t = P.t_start # time starts at t_start y = spring.h() # output of system at start of simulation while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot # updates control and dynamics at faster simulation rate
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') import bobParam as P from signalGenerator import signalGenerator from bobAnimation import bobAnimation from plotData import plotData from bobDynamics import bobDynamics from bobControllerSS import bobControllerSS refsig = signalGenerator(amplitude=0.25/2, frequency=0.1) ctrl = bobControllerSS() bob = bobDynamics() animation = bobAnimation() dataPlot = plotData() t = P.t_start while t < P.t_end: ref_input = refsig.square(t) + 0.25 t_next_plot = t+P.t_plot while t < t_next_plot: F = ctrl.u(ref_input, bob.states()) bob.propagateDynamics(F) t = t+P.Ts
matplotlib.use('TkAgg') import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import massSpringParam as P from signalGenerator import signalGenerator from massSpringAnimation import massSpringAnimation from dataPlotter import dataPlotter from massSpringDynamics import massSpringDynamics from massSpringController import massSpringController # instantiate reference input classes massSpring = massSpringDynamics() controller = massSpringController() reference = signalGenerator(amplitude=4.0, frequency=0.1) zRef = signalGenerator(amplitude=57.0 * np.pi / 180.0, frequency=0.05) fRef = signalGenerator(amplitude=0.25) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = massSpringAnimation() t = P.t_start z = massSpring.h() while t < P.t_end: # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot # updates control and dynamics at faster simulation rate while t < t_next_plot: # Get referenced inputs from signal generators
import matplotlib.pyplot as plt import numpy as np import massParam as P from signalGenerator import signalGenerator from massAnimation import massAnimation from dataPlotter import dataPlotter from massDynamics import massDynamics from controllerObsv import controllerObsv from dataPlotterObserver import dataPlotterObserver # instantiate system, controller, and reference classes mass = massDynamics() controller = controllerObsv() reference = signalGenerator(amplitude=0.5, frequency=0.05) disturbance = signalGenerator(amplitude=0.1) noise = signalGenerator(amplitude=0.01) # instantiate the simulation plots and animation dataPlot = dataPlotter() dataPlotObserver = dataPlotterObserver() animation = massAnimation() t = P.t_start y = mass.h() while t < P.t_end: t_next_plot = t + P.t_plot while t < t_next_plot: r = reference.square(t) d = disturbance.step(t) n = 0.0 #noise.random(t) u, xhat = controller.update(r, y + n)
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import vtolParam as P from signalGenerator import signalGenerator from vtolAnimation import vtolAnimation from plotData import plotData from vtolDynamics import vtolDynamics # instantiate reference input classes vtol = vtolDynamics() reference = signalGenerator(amplitude=0.5, frequency=0.1) zRef = signalGenerator(amplitude=0.5, frequency=0.1) thetaRef = signalGenerator(amplitude=1/4*np.pi, frequency=0.1) flRef = signalGenerator(amplitude=7.3575, frequency=.05) frRef = signalGenerator(amplitude=7.2, frequency=.05) # instantiate the simulation plots and animation dataPlot = plotData() animation = vtolAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables r = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot fr = [] fl = []
def __init__(self): super().__init__() self.m_modVal = [0, 1, 2, 3, 4, 8, 0x24, 0x26, 0x43, 0X46, 0x82] self.fskCounter = 0 self.ofdmCounter = 0 self.oqpskCounter = 0 self.pkt_rssi = 0 self.noiseFloor = 0 self.m_serialPort = serialPort.serialPort() self.m_spectrumAnalyzer = spectrumAnalyzer.spectrumAnalyzer() self.m_signalGenerator = signalGenerator.signalGenerator() self.m_txContainerWidget = QWidget( self) # container for widgets in tx box self.m_connectPort = QPushButton("Connect", self.m_txContainerWidget) self.m_disconnectPort = QPushButton("Disconnect", self.m_txContainerWidget) self.m_disconnectPort.setEnabled(False) self.m_txMod = QComboBox( self.m_txContainerWidget) # tx modulation selection self.psdulen = QLineEdit(self.m_txContainerWidget) # psdu length self.m_txFreq = QLineEdit(self.m_txContainerWidget) # tx frequency self.m_txPower = QComboBox( self.m_txContainerWidget) # tx power selection self.m_numOfPacket = QLineEdit( self.m_txContainerWidget) # tx number of packets self.m_gap = QLineEdit(self.m_txContainerWidget) # tx interpacket gap self.m_txLoopEnable = QCheckBox( "Loop test", self.m_txContainerWidget) # tx loop test section self.m_txFreqOffsetEnable = QCheckBox("Freq offset", self.m_txContainerWidget) self.m_txFreqStop = QLineEdit(self.m_txContainerWidget) self.m_txFreqStep = QLineEdit(self.m_txContainerWidget) self.m_txPowerStop = QComboBox(self.m_txContainerWidget) self.m_txPowerStep = QComboBox(self.m_txContainerWidget) self.m_testMode = QCheckBox("Test mode", self.m_txContainerWidget) # tx test mode self.m_txContinuous = QCheckBox("Continuous", self.m_txContainerWidget) self.m_txCW = QCheckBox("CW", self.m_txContainerWidget) self.m_ant = QComboBox(self.m_txContainerWidget) self.m_txStart = QPushButton("Start TX", self.m_txContainerWidget) self.m_txStop = QPushButton("Stop TX", self.m_txContainerWidget) self.m_txACLR = QPushButton("ACLR test", self.m_txContainerWidget) self.txGroupBox = QGroupBox("Tx", self) self.m_rxContainerWidget = QWidget(self) self.rxGroupBox = QGroupBox("Rx", self) self.m_rxFreq = QLineEdit(self.m_rxContainerWidget) self.m_rxLoopEnable = QCheckBox("Loop test", self.m_rxContainerWidget) self.m_rxTestMode = QCheckBox("Test mode", self.m_rxContainerWidget) self.m_rxFreqStop = QLineEdit(self.m_rxContainerWidget) self.m_rxFreqStep = QLineEdit(self.m_rxContainerWidget) self.m_rxPowerStart = QLineEdit(self.m_rxContainerWidget) self.m_rxPowerStop = QLineEdit(self.m_rxContainerWidget) self.m_rxPowerStep = QLineEdit(self.m_rxContainerWidget) self.m_cableLoss = QLineEdit(self.m_rxContainerWidget) self.m_rssiRequest = QPushButton( "Noise floor", self.m_rxContainerWidget) # RSSI request, disabled by default self.m_rssi = QLineEdit(self.m_rxContainerWidget) self.m_rxConfig = QPushButton( "Start Rx", self.m_rxContainerWidget) # rx config and RSSI loop test self.m_fskCounter = QLineEdit(self.m_rxContainerWidget) self.m_ofdmCounter = QLineEdit(self.m_rxContainerWidget) self.m_oqpskCounter = QLineEdit(self.m_rxContainerWidget) self.m_pkt_rssi = QLineEdit(self.m_rxContainerWidget) self.m_FSK150k = QCheckBox("FSK150k", self.m_rxContainerWidget) self.m_OFDM200k = QCheckBox("OFDM200k", self.m_rxContainerWidget) self.m_OFDM600k = QCheckBox("OFDM600k", self.m_rxContainerWidget) self.m_LR125k = QCheckBox("LR12.5k", self.m_rxContainerWidget) self.m_SSNFSK100k = QCheckBox("SSNFSK100k", self.m_rxContainerWidget) self.m_SSNFSK150k = QCheckBox("SSNFSK150k", self.m_rxContainerWidget) self.m_SSNGFSK150k = QCheckBox("SSNGFSK150k", self.m_rxContainerWidget) self.m_SSNGFSK200k = QCheckBox("SSNGFSK200k", self.m_rxContainerWidget) self.m_SSNGFSK300k = QCheckBox("SSNGFSK300k", self.m_rxContainerWidget) self.m_OFDM1200k = QCheckBox("OFDM1200k", self.m_rxContainerWidget) self.m_OFDM2400k = QCheckBox("OFDM2400k", self.m_rxContainerWidget) self.m_packets = QLineEdit(self.m_rxContainerWidget) self.m_sensitivity = QPushButton("Sensitivity", self.m_rxContainerWidget) self.m_rssiSweep = QPushButton("RSSI sweep", self.m_rxContainerWidget) self.setWindowTitle("RF_PHY") self.createTxBox() self.createRxBox() """ Toolbar section for equipments """ m_toolbars = QToolBar() m_sa_panel = QAction("Spectrum analyzer", self) m_toolbars.addAction(m_sa_panel) m_sa_panel.triggered.connect(lambda: self.m_spectrumAnalyzer.show()) m_sg_panel = QAction("Signal generator", self) m_toolbars.addAction(m_sg_panel) m_toolbars.setMovable(False) m_toolbars.addSeparator() m_sg_panel.triggered.connect(lambda: self.m_signalGenerator.show()) main_layout = QGridLayout() main_layout.addWidget(m_toolbars) main_layout.addWidget(self.txGroupBox, 1, 0) main_layout.addWidget(self.rxGroupBox, 1, 1) self.setLayout(main_layout)
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') import vtolParam as P from signalGenerator import signalGenerator from vtolAnimation import vtolAnimation from plotData_vtol import plotData_vtol from vtolDynamics import vtolDynamics firstsignal = signalGenerator(amplitude=.25, frequency=.6) slowersignal = signalGenerator(amplitude=.25, frequency=.1) vtol = vtolDynamics() choice = int(input('animate(1) or plot(2)?')) if choice == 1: animation = vtolAnimation() elif choice == 2: dataPlot = plotData_vtol() t = P.t_start while t < P.t_end: ref_input = [0, 0, 0] # zv_ref, h_ref, theta_ref t_next_plot = t + P.t_plot while t < t_next_plot: zt = 0 f_l = 7.36 f_r = f_l + .00001 F = [f_l, f_r]
import matplotlib.pyplot as plt import sys sys.path.append('..') # add parent directory import pendulumParam as P from signalGenerator import signalGenerator from pendulumAnimation import pendulumAnimation from plotData import plotData from pendulumDynamics import pendulumDynamics # instantiate pendulum, controller, and reference classes pendulum = pendulumDynamics() reference = signalGenerator(amplitude=0.5, frequency=0.02) force = signalGenerator(amplitude=1, frequency=1) # instantiate the simulation plots and animation dataPlot = plotData() animation = pendulumAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate f = force.sin(t) pendulum.propagateDynamics(f) # Propagate the dynamics t = t + P.Ts # advance time by Ts # update animation and data plots animation.drawPendulum(pendulum.states()) dataPlot.updatePlots(t, ref_input, pendulum.states(), f)
import sys sys.path.append('..') # add parent directory import vtolParam as P from lon_controller import LonController from lat_controller import LatController from signalGenerator import signalGenerator from vtolAnimation import vtolAnimation from plotData import plotData from vtolDynamics import vtolDynamics from plotObserverData import plotObserverData # instantiate reference input classes vtol = vtolDynamics() lon_ctrl = LonController() lat_ctrl = LatController() z_reference = signalGenerator(amplitude=2, frequency=0.15) h_reference = signalGenerator(amplitude=1, frequency=0.08) # set disturbance input force_disturbance = 1.0 torque_disturbance = 0.1 # instantiate the simulation plots and animation dataPlot = plotData() observerPlot = plotObserverData() animation = vtolAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables z_r = z_reference.square(t)
import sys sys.path.append('..') # add parent directory import matplotlib.pyplot as plt # import numpy as np import D_param as P from D_dynamics import D_dynamics from D_controller import D_controller from signalGenerator import signalGenerator from D_animation import D_animation from plotData import plotData # instantiate arm, controller, and reference classes msd = D_dynamics() ctrl = D_controller() reference = signalGenerator(amplitude=1.0, frequency=0.03) # instantiate the simulation plots and animation dataPlot = plotData() animation = D_animation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot while t < t_next_plot: # updates control and dynamics at faster simulation rate u = ctrl.u(ref_input, msd.outputs()) # Calculate the control value msd.propagateDynamics(u) # Propagate the dynamics t = t + P.Ts # advance time by Ts # update animation and data plots
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import vtolParam as P from signalGenerator import signalGenerator from vtolAnimation import vtolAnimation from dataPlotter import dataPlotter # instantiate reference input classes reference = signalGenerator(amplitude=0.5, frequency=0.1) zvRef = signalGenerator(amplitude=0.5, frequency=0.1) ztRef = signalGenerator(amplitude=0.5, frequency=0.1) hRef = signalGenerator(amplitude=0.5, frequency=0.1) thetaRef = signalGenerator(amplitude=.5 * np.pi, frequency=0.5) fRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = vtolAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables r = reference.square(t) zt = ztRef.sin(t) zv = zvRef.sin(t) h = 1 + 2 * hRef.sin(t)**2 theta = thetaRef.square(t) f = fRef.sin(t)
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import satelliteParam as P from signalGenerator import signalGenerator from satelliteAnimation import satelliteAnimation from dataPlotter import dataPlotter # instantiate reference input classes reference = signalGenerator(amplitude=0.5, frequency=0.1) thetaRef = signalGenerator(amplitude=2.0 * np.pi, frequency=0.1) phiRef = signalGenerator(amplitude=0.5, frequency=0.1) tauRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = satelliteAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables r = reference.square(t) theta = thetaRef.sin(t) phi = phiRef.sin(t) tau = tauRef.sawtooth(t) # update animation state = np.array([[theta], [phi], [0.0], [0.0]]) animation.update(state)
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') import vtolParam as P from signalGenerator import signalGenerator from vtolAnimation import vtolAnimation from plotData_vtol import plotData_vtol from vtolDynamics import vtolDynamics from vtolControllerSS import vtolControllerSS ctrl = vtolControllerSS() vtol = vtolDynamics() refsig = signalGenerator(amplitude=1.5, frequency=0.06) animation = vtolAnimation() dataPlot = plotData_vtol() t = P.t_start while t < P.t_end: zv_ref_input = refsig.square(t) + 3 h_ref_input = .5 * refsig.square(2.0 * t) + 1.5 reference = [zv_ref_input, h_ref_input] t_next_plot = t + P.t_plot while t < t_next_plot: Forces = ctrl.u(reference, vtol.states()) vtol.propagateDynamics(Forces) t = t + P.Ts
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') import msdParam as P from signalGenerator import signalGenerator from msdAnimation import msdAnimation from plotData_msd import plotData_msd ## start up the signalGenerator(s) firstsignal = signalGenerator(amplitude=0.1, frequency=.6) ## start up the plotData class and the msdAnimation class dataPlot = plotData_msd() animation = msdAnimation() ## main animation and simulation loop t = P.t_start while t < P.t_end: ## location of the MSD (z) is generated z = firstsignal.sin(t) ## update the animation animation.drawMSD([z]) ## update the data plots dataPlot.updatePlots(t, z, [z], z) ## increment time and pause t = t + P.t_plot
import matplotlib.pyplot as plt import numpy as np import sys sys.path.append('..') # add parent directory import VTOLParam as P from signalGenerator import signalGenerator from VTOLAnimation import VTOLAnimation from dataPlotter import dataPlotter # instantiate reference input classes reference = signalGenerator(amplitude=1.0, frequency=0.05, y_offset=0.1) thetaRef = signalGenerator(amplitude=1.0, frequency=0.05, y_offset=0.1) phiRef = signalGenerator(amplitude=1.0, frequency=0.05, y_offset=0.5) tauRef = signalGenerator(amplitude=5, frequency=.5) # instantiate the simulation plots and animation dataPlot = dataPlotter() animation = VTOLAnimation() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # set variables r = reference.sin(t) theta = thetaRef.sin(t) phi = phiRef.sin(t) tau = tauRef.sawtooth(t) # update animation state = np.array([[r], [phi], [theta], [0.0]]) animation.update(state, 0.0)
import sys sys.path.append('..') # add parent directory import matplotlib.pyplot as plt import pendulumParam as P from pendulumDynamics import pendulumDynamics from pendulumController import pendulumController from signalGenerator import signalGenerator from pendulumAnimation import pendulumAnimation from plotData import plotData from plotObserverData import plotObserverData # instantiate pendulum, controller, and reference classes pendulum = pendulumDynamics() ctrl = pendulumController() reference = signalGenerator(amplitude=0.5, frequency=0.05) # set disturbance input disturbance = 0.5 # instantiate the simulation plots and animation dataPlot = plotData() animation = pendulumAnimation() observerPlot = plotObserverData() t = P.t_start # time starts at t_start while t < P.t_end: # main simulation loop # Get referenced inputs from signal generators ref_input = reference.square(t) # Propagate dynamics in between plot samples t_next_plot = t + P.t_plot
def _getSignalList(self): self.signal = signalG.signalGenerator(self.signalParam) self.signalGenerator = self.signal.generateSignals() self.data.calcSignals(self.signalGenerator) pass