Help in Self Balancing bot

[SwapnilSonawane894]

Sir i want help in my blancing bot could you please help me

[gauravsethia08]

Sure, can you give me more info about what are you working on, and how i can be of help?

[SwapnilSonawane894]

Sir I am working on self-balancing bot I am trying to balance the bot using LQR control technique I am using Coppelia Sim for it with python. I am facing problems for finding the LQR gain tried all possible solution could please help 🥺

[SwapnilSonawane894]

@gauravsethia08 Please reply, Please help me with these project.

[gauravsethia08]

Could you provide a link to your code base and mention whats issue you are facing?

[SwapnilSonawane894]

@gauravsethia08 ok sir

import numpy as np
import time

def sysCall_init():
    global velocity, yaw_velocity, last_key_time, radius
    sim = require('sim')
    math = require('math')

    # Initialization
    self.body = sim.getObject("/body")
    self.rightJoint = sim.getObject("/right_joint")
    self.leftJoint = sim.getObject("/left_joint")
    self.floor = sim.getObject("/Floor")
    self.K = np.array([1.4142135623731116, 1.6324576461398326, 0.8958387217853694, 0.01639766421577393])
    self.accel = 0.0
    self.turn = 0.0
    self.damping_factor = 2.5  # Adjust as needed
    velocity = 0.0  # Initialize global velocity
    yaw_velocity = 0.0  # Initialize global yaw velocity
    radius = 0.0215  # Radius of the wheel (m)
    last_key_time = time.time()  # Initialize last key press time

def saturate(value, min_value, max_value):
    return max(min(value, max_value), min_value)

def control():
    global velocity, yaw_velocity, last_key_time
    message, data, data2 = sim.getSimulatorMessage()
    current_time = time.time()

    if message == sim.message_keypress:
        if current_time - last_key_time > 0.1:  # Adjust debounce time as needed
            if data[0] == 2007:  # forward up arrow
                velocity = 20.0  # Set speed forward
            elif data[0] == 2008:  # backward down arrow
                velocity = -20.0  # Set speed backward
            elif data[0] == 2009:  # left arrow key (yaw left)
                yaw_velocity = 2.0  # Set yaw left and reduced impact
            elif data[0] == 2010:  # right arrow key (yaw right)
                yaw_velocity = -2.0  # Set yaw right and reduced impact
            last_key_time = current_time
    else:
        velocity = 0.0  # Stop when no key pressed
        yaw_velocity = 0.0

def sysCall_actuation():
    global velocity, yaw_velocity, radius
    # Get the current state
    objectMat = sim.getObjectMatrix(self.body, self.floor)
    unitX = objectMat[1:10:4]
    unitY = objectMat[2:11:4]
    unitZ = objectMat[3:12:4]

    rollDeg = np.degrees(np.arctan2(unitX[2], unitZ[2]))  # Roll angle
    pErr = np.degrees(np.arctan2(-unitY[2], unitZ[2]))    # Pitch angle
    xErr = sim.getObjectPosition(self.body, self.floor)[1]  # Position error

    # Get velocity (linear and angular) from simulation
    linearVelocity, angularVelocity = sim.getObjectVelocity(self.body, self.floor)

    # State vector [position, velocity, angle, angular velocity]
    state = np.array([xErr, linearVelocity[1], rollDeg, angularVelocity[2]])

    # Compute control input using LQR with damping factor
    u = -np.dot(self.K, state) * self.damping_factor

    # Convert torque to angular velocity
    angular_velocity_right = (u + self.accel + velocity - yaw_velocity) / radius
    angular_velocity_left = (u + self.accel + velocity + yaw_velocity) / radius

    # Cap the velocities to prevent excessive values
    angular_velocity_right = saturate(angular_velocity_right, -10, 10)  # Adjust limits as needed
    angular_velocity_left = saturate(angular_velocity_left, -10, 10)  # Adjust limits as needed

    # Apply control input to the joints
    sim.setJointTargetVelocity(self.rightJoint, angular_velocity_right)
    sim.setJointTargetVelocity(self.leftJoint, angular_velocity_left)

def sysCall_sensing():
    control()

def sysCall_cleanup():
    pass

just paseted here, i am not sure my lqr gain is correct or not, could you please help in thes for finding the gain for smooth balance in the bot ( i am using coppelia simulation for these )