Untitled

# ------------------------------------- AVAILABLE FUNCTIONS --------------------------------#
# qube.setRGB(r, g, b) - Sets the LED color of the QUBE. Color values range from [0, 999].
# qube.setMotorSpeed(speed) - Sets the motor speed. Speed ranges from [-999, 999].
# qube.setMotorVoltage(volts) - Applies the given voltage to the motor. Volts range from (-24, 24).
# qube.resetMotorEncoder() - Resets the motor encoder in the current position.
# qube.resetPendulumEncoder() - Resets the pendulum encoder in the current position.

# qube.getMotorPosition() - Returns the cumulative angular positon of the motor.
# qube.getPendulumPosition() - Returns the cumulative angular position of the pendulum.
# qube.getMotorRPM() - Returns the newest rpm reading of the motor.
# qube.getMotorCurrent() - Returns the newest reading of the motor's current.
# ------------------------------------- AVAILABLE FUNCTIONS --------------------------------#

from QUBE import *
from logger import *
from com import *
from liveplot import *
from time import time
import threading
from PID import *
import numpy as np
from observer import observer  # Import the observer class

# Replace with the Arduino port. Can be found in the Arduino IDE (Tools -> Port:)
port = "COM5"
baudrate = 115200
qube = QUBE(port, baudrate)

# Resets the encoders in their current position.
qube.resetMotorEncoder()
qube.resetPendulumEncoder()

# Enables logging - comment out to remove
enableLogging()

t_last = time()

m_target = 0
p_target = 0
#observer k values
k1 = 1.9985 
k2 = 0.2033
#positon control
k1p = 0.0328
k2p = -0.013
#velocity control
k1v = 0
k2v =  0.018
k3v = -0.16
error_integral = 0

pid = PID(kp, ki, kd)
obs = observer()  # Instantiate the observer object

def control(data, lock):
    global m_target, p_target, pid, obs,error_integral
    v_ref = 1000  # Desired reference speed

    while True:
        
        # Updates the QUBE - Sends and receives data
        qube.update()
        dt = getDT()

        # Gets the logdata and writes it to the log file
        logdata = qube.getLogData(m_target, p_target)
        save_data(logdata)

        with lock:
            doMTStuff(data)

        ## PID controller implementation

        #actualValue = qube.getMotorAngle() 
        #u = pid.regulate(500 , actualValue)

        ##state-controller implementation

        x1 = qube.getMotorAngle()  
        x2 = qube.getMotorRPM() 
        state_error = -v_ref + x2 
        error_integral += state_error * dt 
        u = -k1p*x1 + k2p*state_error # position control 
        uv =-k1v*x1 - k2v*state_error + k3v*error_integral # velocity control

        qube.setMotorVoltage(u) # postion control
        #qube.setMotorVoltage(uv) # velocity control
        print(v_ref)
        print(x2) 

        ##observer implementation

        error = v_ref - x1  
        
        u = -k1 * x1 - k2 * error 
        u = max(min(u, 24), -24)

        # Estimate the speed using the observer
        speed = obs.regulate(x1, u, x2)

        qube.setMotorVoltage(u)
        print("Reference Speed:", v_ref)
        print("Measured Speed:", x2) 
        print("Control Input:", u)
        print("Estimated Speed:", speed)  # Print the estimated speed
        
firstCallFlag = True 
def getDT():
    global t_last, firstCallFlag  
    if firstCallFlag:
        firstCallFlag = False  
        t_last = time()
    t_now = time()
    dt = t_now - t_last
    t_last = t_now  
    return dt


def doMTStuff(data):
    packet = data[7]
    pid.copy(packet.pid)
    if packet.resetEncoders:
        qube.resetMotorEncoder()
        qube.resetPendulumEncoder()
        packet.resetEncoders = False

    new_data = qube.getPlotData(m_target, p_target)
    for i, item in enumerate(new_data):
        data[i].append(item)



if __name__ == "__main__":
    _data = [[], [], [], [], [], [], [], Packet()]
    lock = threading.Lock()
    thread1 = threading.Thread(target=startPlot, args=(_data, lock))
    thread2 = threading.Thread(target=control, args=(_data, lock))
    thread1.start()
    thread2.start()
    thread1.join()
    thread2.join()