class RobotThread(threading.Thread):
def __init__(self,
group=None,
target=None,
name=None,
args=(),
kwargs=None,
verbose=None):
super(RobotThread, self).__init__(group=group,
target=target,
name=name)
self.args = args
self.kwargs = kwargs
self._continueFlag = True # flag to continue running the thread
self._resetFlag = False # flag to reset everything
self._runControl = False # process control
# Initialize the pigpio library
self._pi = pigpio.pi()
# Initialize all hardware
self.InitMPU6050()
self.InitMotors()
self.InitControl()
# Store current time
self._currTimePrev = time.time()
# Sleep time for control loop
self._sleepTime = 0.1
return
@property
def CtrlErr(self):
return self._ctrlErr
@property
def CtrlErrRate(self):
return self._ctrlErrRate
@property
def CtrlOutput(self):
return self._ctrlOutput
@property
def Kp(self):
return self._P
@Kp.setter
def Kp(self, val):
self._P = val
self._pid.setKp(val)
@property
def Ki(self):
return self._I
@Ki.setter
def Ki(self, val):
self._I = val
self._pid.setKi(val)
@property
def Kd(self):
return self._D
@Kd.setter
def Kd(self, val):
self._D = val
self._pid.setKd(val)
def InitMPU6050(self):
# Accelerometer Values and Device
self._mpuIdx = ['x', 'y', 'z']
self._a = [0.0, 0.0, 0.0] # Acceleration (g's)
self._w = [0.0, 0.0, 0.0] # Angular rate (deg/s)
self._wFilt = [0.0, 0.0, 0.0] # Filtered Angular rate (deg/s)
self._theta = [0.0, 0.0, 0.0] # Angle (deg)
self._thetaFilt = [0.0, 0.0, 0.0] # Filtered Angle (deg)
self._a_off = [0.66, -0.26, 10.22 - 9.81]
self._w_off = [-1.07, 0.67, -1.55]
#self._a_off = [0.0, 0.0, 0.0]
#self._w_off = [0.0, 0.0, 0.0]
self._thetaXFilt = ComplementaryFilter(alpha=0.05) # alpha = 0.02
self._thetaXFilt2 = Filter(timeConst=0.0)
self._wXFilt = Filter(timeConst=0.0)
self._mpu6050 = mpu6050(0x68) # 0x68 - default I2C slave addr
self._mpu6050.set_accel_range(mpu6050.ACCEL_RANGE_2G)
self._mpu6050.set_gyro_range(mpu6050.GYRO_RANGE_250DEG)
return
def InitMotors(self):
# Motor Controllers
# Pins - PWM, FWD, REV
#self._motorLookupV = [0, 12]
#self._motorLookupPWM = [0, 100]
self._motorLookupV = [0.0, 1.0, 1.1, 10.4, 11.0, 11.5, 11.8, 12.0]
self._motorLookupPWM = [0.0, 1.0, 2.0, 80.0, 85.0, 90.0, 95.0, 100.0]
self._motorLookup = LookupTable(\
self._motorLookupV, self._motorLookupPWM)
self._motorLeft = MotorControl_pigpio(\
self._pi, 13, 19, 26)
self._motorRight = MotorControl_pigpio(\
self._pi, 16, 20, 21)
self._motorLeft.SetVoltageLookupTable(\
self._motorLookup)
self._motorRight.SetVoltageLookupTable(\
self._motorLookup)
# Motor Encoders
# TODO
return
def InitControl(self):
self._P = 0.8
self._I = 14.0
self._D = 0.12
self._maxIntegralOutput = 9.0
self._pid = PID(self._P, self._I, self._D)
self._pid.setWindup(100.0)
self._ctrlOutputMin = 1.0 # Volts, min motor output
self._ctrlOutputMax = 12.0 # Volts, max motor output
return
def ProcessMPU6050(self):
# Get the raw data
self._mpu6050AccelData = self._mpu6050.get_accel_data()
self._mpu6050GyroData = self._mpu6050.get_gyro_data()
#self._mpu6050AccelData = {'x': 0.0, 'y': 0.0, 'z': 9.81}
#self._mpu6050GyroData = {'x': 0.0, 'y': 0.0, 'z': 0.0}
# Subtract out calibration offsets
for i in range(0, 3): # (0, 1, 2)
self._a[i] = self._mpu6050AccelData[self._mpuIdx[i]] - \
self._a_off[i]
self._w[i] = self._mpu6050GyroData[self._mpuIdx[i]] - \
self._w_off[i]
# Calculate angle from accelerometer data
self._theta[0] = math.degrees( \
math.atan2(self._a[1], self._a[2])) # atan2(ay, az)
# Complementary filter on accel and gyro data
thetaFilt = self._thetaXFilt.Filter(self._dT, self._w[0],
self._theta[0])
# Filter the resulting angle
self._thetaFilt[0] = self._thetaXFilt2.Filter(self._dT, thetaFilt)
# Filter the angular velocity for controller deriviative term
self._wFilt[0] = self._wXFilt.Filter(self._dT, self._w[0])
# If the robot has tipped over, stop trying to control
if math.fabs(self._thetaFilt[0]) > 30.0:
self._runControl = False
return
def ProcessControl(self):
# Calculate the control error and rate
self._ctrlErr = self._thetaFilt[0]
self._ctrlErrRate = -self._wFilt[0]
# Adjust the max integral windup
if self._pid.Ki > 0.0:
self._pid.setWindup(self._maxIntegralOutput / self._pid.Ki)
# Run the PID controller
self._ctrlOutput = self._pid.update(self._ctrlErr, self._ctrlErrRate)
# Control saturation
if self._ctrlOutput > self._ctrlOutputMax:
self._ctrlOutput = self._ctrlOutputMax
if self._ctrlOutput < -self._ctrlOutputMax:
self._ctrlOutput = -self._ctrlOutputMax
# Clear integrator if not running
if not self._runControl:
self._pid.ITerm = 0.0
return
def ProcessMotors(self):
# If not running control, set both motors to brake
if not self._runControl:
self._motorLeft.Brake()
self._motorRight.Brake()
return
# Set directions for left and right motors
self._ctrlLeft = self._ctrlOutput
self._ctrlRight = -self._ctrlOutput
# Write the motor control signal
self._motorLeft.SetVoltage(self._ctrlLeft)
self._motorRight.SetVoltage(self._ctrlRight)
# Process feedback from encoders
# TODO
return
def run(self):
while self._continueFlag:
#print self._runControl
# Calculate time delta
self._currTime = time.time()
self._dT = self._currTime - self._currTimePrev
self._currTimePrev = self._currTime
# Read accelerometer and gyro data and process
self.ProcessMPU6050()
# Run PID Controller
self.ProcessControl()
# Run motor output
self.ProcessMotors()
#logging.debug('running with %s and %s', self.args, self.kwargs)
#time.sleep(self._sleepTime)
# Stop condition
self._pi.stop()
return
def StartControl(self):
self._runControl = True
def StopControl(self):
self._runControl = False
def Stop(self):
self._continueFlag = False
def Reset(self):
self._resetFlag = False
def ProcessCommands(self, cmd, conn):
#print cmd
next = 0
if cmd[0] == "CMD":
if cmd[1] == "START":
t.StartControl()
next = 2
if cmd[1] == "STOP":
t.StopControl()
next = 2
if cmd[0] == "GET":
if cmd[1] == "GAINS":
resp = "RESP GAINS {0} {1} {2} ".format(
self.Kp, self.Ki, self.Kd)
conn.send(resp)
next = 2
if cmd[1] == "PARAMS":
resp = "RESP PARAMS {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} ".format(\
self.CtrlErr, self.CtrlErrRate, self.CtrlOutput, \
self._pid.PTerm, self._pid.ITerm * self._pid.Ki, self._pid.DTerm * self._pid.Kd)
conn.send(resp)
next = 2
if cmd[0] == "SET":
if cmd[1] == "GAINS":
t.Kp = float(cmd[2])
t.Ki = float(cmd[3])
t.Kd = float(cmd[4])
resp = "RESP GAINS {0} {1} {2} ".format(
self.Kp, self.Ki, self.Kd)
conn.send(resp)
next = 5
if (next < len(cmd)) and (next > 0):
self.ProcessCommands(cmd[next:], conn)
