Ejemplo n.º 1
0
    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)
Ejemplo n.º 3
0
    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())
Ejemplo n.º 4
0
	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")
Ejemplo n.º 5
0
def set_motors(s1, s2):
    go.motor1(1, s1)
    go.motor2(1, s2)
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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")
Ejemplo n.º 8
0
def motor2_callback(data):
    direction = data.data[0]
    speed = data.data[1]
    v = gopigo.motor2(direction,speed)
Ejemplo n.º 9
0
def stop_motors():
    gopigo.motor1(0,0)
    gopigo.motor2(0,0)
Ejemplo n.º 10
0
def motor2_callback(data):
    direction = data.data[0]
    speed = data.data[1]
    v = gopigo.motor2(direction, speed)
Ejemplo n.º 11
0
def stop_motors():
    gopigo.motor1(0, 0)
    gopigo.motor2(0, 0)
Ejemplo n.º 12
0
def set_motors(speed1, speed2):
    go.motor1(1, speed1)
    go.motor2(1, speed2)
Ejemplo n.º 13
0
def motor2(kargs):
    r = {'return_value': gopigo.motor2(int(kargs['direction']), int(kargs['speed']))}
    return r