class motor_control():
def __init__(self):
#pid loop for each motor
self.pid0 = PID(P=0.0001, I=0.00001)
self.pid1 = PID(P=0.0001, I=0.00001)
self.high_power_limit = 0.70
self.low_power_limit = 0.15
self.low_rpm_limit = 250
self.deadband = 0.1
self.pub = rospy.Publisher('motor_power', MotorPower, queue_size=10)
self.motor_power = MotorPower()
self.motor_power.power1 = 0
self.motor_power.power2 = 0
def set_motor_power(self):
self.motor_power.power1 = self.pid0.PID + self.motor_power.power1
self.motor_power.power2 = self.pid1.PID + self.motor_power.power2
if self.pid0.getPoint() > 0 and self.pid0.getPoint(
) < self.low_rpm_limit:
self.motor_power.power1 = self.low_power_limit
elif self.pid0.getPoint() < 0 and self.pid0.getPoint(
) > -self.low_rpm_limit:
self.motor_power.power1 = -self.low_power_limit
elif self.pid0.getPoint() == 0:
self.motor_power.power1 = 0
elif self.motor_power.power1 > self.high_power_limit:
self.motor_power.power1 = self.high_power_limit
elif self.motor_power.power1 < self.low_power_limit:
# if self.motor_power.power1 > -self.low_power_limit+self.deadband and self.motor_power.power1 < self.low_power_limit-self.deadband:
# self.motor_power.power1=0
if self.motor_power.power1 < 0 and self.motor_power.power1 > -self.low_power_limit:
self.motor_power.power1 = -self.low_power_limit
elif self.motor_power.power1 < -self.high_power_limit:
self.motor_power.power1 = -self.high_power_limit
else:
self.motor_power.power1 = self.low_power_limit
if self.pid1.getPoint() > 0 and self.pid1.getPoint(
) < self.low_rpm_limit:
self.motor_power.power2 = self.low_power_limit
elif self.pid1.getPoint() < 0 and self.pid1.getPoint(
) > -self.low_rpm_limit:
self.motor_power.power2 = -self.low_power_limit
elif self.pid1.getPoint() == 0:
self.motor_power.power2 = 0
elif self.motor_power.power2 > self.high_power_limit:
self.motor_power.power2 = self.high_power_limit
elif self.motor_power.power2 < self.low_power_limit:
if self.motor_power.power2 < 0 and self.motor_power.power2 > -self.low_power_limit:
self.motor_power.power2 = -self.low_power_limit
elif self.motor_power.power2 < -self.high_power_limit:
self.motor_power.power2 = -self.high_power_limit
else:
self.motor_power.power2 = self.low_power_limit
class motor_control():
def __init__(self):
#pid loop for each motor
self.pid0=PID(P=0.0001, I=0.00001)
self.pid1=PID(P=0.0001, I=0.00001)
self.high_power_limit=0.70
self.low_power_limit=0.15
self.low_rpm_limit=250
self.deadband=0.1
self.pub = rospy.Publisher('motor_power', MotorPower, queue_size=10)
self.motor_power=MotorPower()
self.motor_power.power1=0
self.motor_power.power2=0
def set_motor_power(self):
self.motor_power.power1=self.pid0.PID+self.motor_power.power1
self.motor_power.power2=self.pid1.PID+self.motor_power.power2
if self.pid0.getPoint() > 0 and self.pid0.getPoint() < self.low_rpm_limit:
self.motor_power.power1=self.low_power_limit
elif self.pid0.getPoint() < 0 and self.pid0.getPoint() > -self.low_rpm_limit:
self.motor_power.power1=-self.low_power_limit
elif self.pid0.getPoint() == 0:
self.motor_power.power1=0
elif self.motor_power.power1 > self.high_power_limit:
self.motor_power.power1=self.high_power_limit
elif self.motor_power.power1 < self.low_power_limit:
# if self.motor_power.power1 > -self.low_power_limit+self.deadband and self.motor_power.power1 < self.low_power_limit-self.deadband:
# self.motor_power.power1=0
if self.motor_power.power1 < 0 and self.motor_power.power1 > -self.low_power_limit:
self.motor_power.power1 = -self.low_power_limit
elif self.motor_power.power1 < -self.high_power_limit:
self.motor_power.power1= -self.high_power_limit
else:
self.motor_power.power1=self.low_power_limit
if self.pid1.getPoint() > 0 and self.pid1.getPoint() < self.low_rpm_limit:
self.motor_power.power2=self.low_power_limit
elif self.pid1.getPoint() < 0 and self.pid1.getPoint() > -self.low_rpm_limit:
self.motor_power.power2=-self.low_power_limit
elif self.pid1.getPoint() == 0:
self.motor_power.power2=0
elif self.motor_power.power2 > self.high_power_limit:
self.motor_power.power2=self.high_power_limit
elif self.motor_power.power2 < self.low_power_limit:
if self.motor_power.power2 < 0 and self.motor_power.power2 > -self.low_power_limit:
self.motor_power.power2 = -self.low_power_limit
elif self.motor_power.power2 < -self.high_power_limit:
self.motor_power.power2= -self.high_power_limit
else:
self.motor_power.power2=self.low_power_limit
class Roll(object):
def __init__(self, quad):
self.quad = quad
self.command_roll = 0
self.roll = PID(0.02, 0.001, 0.1)
self.roll.setPoint(self.command_roll)
def step(self):
print self.command_roll
if self.roll.getPoint() != self.command_roll:
self.roll.setPoint(self.command_roll)
pforce = self.roll.update(self.quad.roll)
self.quad.motors_force = (0, 0, -pforce, pforce)
self.quad.step()
class Roll(object):
def __init__(self, quad):
self.quad = quad
self.command_roll = 0
self.roll = PID(0.02, 0.001, 0.1)
self.roll.setPoint(self.command_roll)
def step(self):
print self.command_roll
if self.roll.getPoint() != self.command_roll:
self.roll.setPoint(self.command_roll)
pforce = self.roll.update(self.quad.roll)
self.quad.motors_force = (0, 0, -pforce, pforce)
self.quad.step()
class BaseController:
"""
coordinates motor commands with imu odometry to follow velocity commands
"""
def __init__(self):
# Set PID structures to intial values
self.rightPID = PID(-200, 0, 0, 0, 0, 0, 0)
self.rightPID.setPoint(0)
self.leftPID = PID(200, 0, 0, 0, 0, 0, 0)
self.leftPID.setPoint(0)
self.somethingWentWrong = False
# Ensure that data is fresh from both wheels
self.newDataM1 = False
self.newDataM2 = False
self.newData = False
# IMU structures
self.currentLeftV = MovingAverage()
self.currentRightV = MovingAverage()
# Roboclaw
self.myRoboclaw = RoboClaw()
# ROS subscribers
rospy.Subscriber("/cmd_vel", Twist, self.commandCallback)
rospy.Subscriber("/imu1", Imu, self.imu1Callback)
rospy.Subscriber("/imu2", Imu, self.imu2Callback)
time.sleep(1)
self.lastDataTime = time.time()
self.repeatCount = 0
self.currentM1Measurement = 0
self.currentM2Measurement = 0
self.lastM1Measurement = 0
self.lastM2Measurement = 0
def imu1Callback(self,data):
self.newDataM1 = True
self.currentM1Measurement = data.angular_velocity.z
self.currentLeftV.add(data.angular_velocity.z)
def imu2Callback(self,data):
self.newDataM2 = True
self.currentM2Measurement = data.angular_velocity.z
self.currentRightV.add(data.angular_velocity.z)
def commandCallback(self, data):
"""
Will set the internal PID controller's setpoints according to the command
"""
print "****NEW COMMAND RECEIVED***"
linearV=data.linear.x
angularV=data.angular.z
# calculate desired velocities for wheels see Georgia Tech mobile robot 2.2 video for equation
L=.57
R=.9424
self.leftPID.setPoint( -(2*linearV - angularV*L)/(2*R))
self.rightPID.setPoint( (2*linearV + angularV*L)/(2*R))
def shutdown(self):
"""
To be called if something went wrong, stop the roboclaw and exit out
"""
print "shutting down"
self.myRoboclaw.writeM1M2(0,0)
exit()
def commandLoop(self):
"""
Main loop that writes the commands to the roboclaw. If new data is received
at a rate lower than 4Hz, then the wheelchair will shutdown
"""
# Thigns to check:
# 1) We got new data at the correct frequency
# 2) We are not getting repeat data from our imus (imus are still working)
# run loop 20 times a second
newLeft = 0
newRight = 0
r = rospy.Rate(30)
while not rospy.is_shutdown():
# Wait until we get new data from both motors
if self.newDataM1 and self.newDataM2:
self.newData = True
if not self.newData:
if time.time() - self.lastDataTime > .5:
print "Too long between messages!"
self.shutdown()
else:
continue
# We have data, ensure that it isnt repeated data
else:
self.lastDataTime = time.time()
self.newDataM1 = False
self.newDataM2 = False
self.newData = False
# Check the number of repeat readings
if self.currentM1Measurement == self.lastM1Measurement or self.currentM2Measurement == self.lastM2Measurement:
self.repeatCount +=1
else:
self.repeatCount = 0
# If we have too many repeats, shut the system down
if self.repeatCount > 4:
print "Too many repeats!"
self.shutdown()
self.lastM1Measurement= self.currentM1Measurement
self.lastM2Measurement= self.currentM2Measurement
newLeft = newLeft + int(self.leftPID.update(self.currentLeftV.get()))
newRight = newRight + int(self.rightPID.update(self.currentRightV.get()))
#catch special case of not moving
if self.leftPID.getPoint() == 0 and self.rightPID.getPoint() == 0:
newLeft = 0
newRight = 0
if abs(newLeft) > 0:
print "LEFT setpoint, measurement, update",'%1.2f' % self.leftPID.getPoint(), '%1.2f' % self.currentLeftV.get(), '%1.2f' % newLeft, '%1.2f' % int(self.leftPID.update(self.currentLeftV.get()))
print "RIGHT setpoint, measurement, update",'%1.2f' % self.rightPID.getPoint(), '%1.2f' % self.currentRightV.get(),'%1.2f' % newRight, '%1.2f' % int(self.rightPID.update(self.currentRightV.get()))
self.myRoboclaw.writeM1M2(newRight,newLeft) # MAKE SURE THESE ARE IN THE CORRECT ORDER!!!!
r.sleep()
