def start(self):
logger.infof('Gopigo start: {}', self.node_name)
rospy.spin()
motor1(0, 0)
motor2(0, 0)
logger.infof('Gopigo stop: {}', self.node_name)
def motorcmd_2_robot(self, wheel='left', motor_command=0):
if self.gopigo_on:
motor_command_raw = int(abs(motor_command))
import gopigo
if wheel == 'left':
if motor_command >= 0: gopigo.motor1(1, motor_command_raw)
elif motor_command < 0: gopigo.motor1(0, motor_command_raw)
if wheel == 'right':
if motor_command >= 0: gopigo.motor2(1, motor_command_raw)
elif motor_command < 0: gopigo.motor2(0, motor_command_raw)
def _on_receive(self, data):
logger.debugf('received message: {}', data)
t = data.linear.x
r = data.angular.z
logger.debugf('t={}, r={}', t, r)
mag = math.sqrt(pow(t, 2) + pow(r, 2))
if mag < 0.1:
logger.debugf(
'too small value, linear.x: {}, angular.z: {}, mag: {}', t, r,
mag)
motor1(0, 0)
motor2(0, 0)
logger.debugf('moter speed: {}', read_motor_speed())
return
logger.debugf('mag={}', mag)
m1 = t / mag * abs(t)
m2 = t / mag * abs(t)
logger.debugf('m1={}, m2={}', m1, m2)
m1 += r / mag * abs(r)
m2 += -r / mag * abs(r)
logger.debugf('m1={}, m2={}', m1, m2)
if m1 != 0:
m1 = m1 / abs(m1) * min(abs(m1), 1.0)
if m2 != 0:
m2 = m2 / abs(m2) * min(abs(m2), 1.0)
logger.debugf('m1={}, m2={}', m1, m2)
motor1(m1 >= 0, abs(int(m1 * 255)))
motor2(m2 >= 0, abs(int(m2 * 255)))
logger.debugf('moter speed: {}', read_motor_speed())
1) # It is better to stop for a second before changing direction since it leads to less wear on the motors and less load on the power supply
print("Motor 1 moving back at full speed")
gopigo.motor1(0, 255)
time.sleep(2)
print("Motor 1 moving back at half speed")
gopigo.motor1(0, 127)
time.sleep(2)
print("Motor 1 stopping")
gopigo.motor1(1, 0)
# Motor 2 control
print("Motor 2 moving forward at full speed")
gopigo.motor2(1, 255)
time.sleep(2)
print("Motor 2 moving forward at half speed")
gopigo.motor2(1, 127)
time.sleep(2)
print("Motor 2 stopping ")
gopigo.motor2(1, 0)
time.sleep(1)
print("Motor 2 moving back at full speed")
gopigo.motor2(0, 255)
time.sleep(2)
print("Motor 2 moving back at half speed")
def set_motors(s1, s2):
go.motor1(1, s1)
go.motor2(1, s2)
import gopigo as go
def set_motors(s1, s2):
go.motor1(1, s1)
go.motor2(1, s2)
lwt = go.enc_read(0)
rwt = go.enc_read(1)
target_speed = input("Speed?")
print "Starting"
lws = target_speed
rws = target_speed
set_motors(target_speed, target_speed)
while True:
lwt -= go.enc_read(0)
rwt -= go.enc_read(1)
error = rwt - lwt
lws += error / 2
rws += error / 2
if error > 0:
if rws == target_speed:
lws += 2
go.motor1(1, lws)
else:
rws -= 2
go.motor2(1, rws)
time.sleep(1) # It is better to stop for a second before changing direction since it leads to less wear on the motors and less load on the power supply
print("Motor 1 moving back at full speed")
gopigo.motor1(0,255)
time.sleep(2)
print("Motor 1 moving back at half speed")
gopigo.motor1(0,127)
time.sleep(2)
print("Motor 1 stopping")
gopigo.motor1(1,0)
#Motor 2 control
print("Motor 2 moving forward at full speed")
gopigo.motor2(1,255)
time.sleep(2)
print("Motor 2 moving forward at half speed")
gopigo.motor2(1,127)
time.sleep(2)
print("Motor 2 stopping ")
gopigo.motor2(1,0)
time.sleep(1)
print("Motor 2 moving back at full speed")
gopigo.motor2(0,255)
time.sleep(2)
print("Motor 2 moving back at half speed")
def motor2_callback(data):
direction = data.data[0]
speed = data.data[1]
v = gopigo.motor2(direction,speed)
def stop_motors():
gopigo.motor1(0,0)
gopigo.motor2(0,0)
def motor2_callback(data):
direction = data.data[0]
speed = data.data[1]
v = gopigo.motor2(direction, speed)
def stop_motors():
gopigo.motor1(0, 0)
gopigo.motor2(0, 0)
def set_motors(speed1, speed2):
go.motor1(1, speed1)
go.motor2(1, speed2)
def motor2(kargs):
r = {'return_value': gopigo.motor2(int(kargs['direction']), int(kargs['speed']))}
return r
