def turnLeft(self):
if self.interface == "I2C":
self.front_right.move(self.speed, self.accel)
self.rear_left.move(-self.speed, self.accel)
self.front_left.move(-self.speed, self.accel)
self.rear_right.move(self.speed, self.accel)
elif self.interface == "UDP":
motor_udp.send_motor_speed_fr(self.speed)
motor_udp.send_motor_speed_rl(-self.speed)
motor_udp.send_motor_speed_fl(-self.speed)
motor_udp.send_motor_speed_rr(self.speed)
self.state = MotorState.LEFT
def moveBackward(self):
if self.interface == "I2C":
self.front_left.move(-self.speed, self.accel)
self.rear_left.move(-self.speed, self.accel)
self.front_right.move(-self.speed, self.accel)
self.rear_right.move(-self.speed, self.accel)
elif self.interface == "UDP":
motor_udp.send_motor_speed_fl(-self.speed)
motor_udp.send_motor_speed_rl(-self.speed)
motor_udp.send_motor_speed_fr(-self.speed)
motor_udp.send_motor_speed_rr(-self.speed)
self.state = MotorState.BACKWARD
def moveForwardDualSpeed(self, left_speed, right_speed):
left_speed = self.__clipSpeedValue(left_speed)
right_speed = self.__clipSpeedValue(right_speed)
if self.interface == "I2C":
self.front_right.move(right_speed, self.accel)
self.front_left.move(left_speed, self.accel)
self.rear_right.move(right_speed, self.accel)
self.rear_left.move(left_speed, self.accel)
elif self.interface == "UDP":
motor_udp.send_motor_speed_fr(right_speed)
motor_udp.send_motor_speed_fl(left_speed)
motor_udp.send_motor_speed_rr(right_speed)
motor_udp.send_motor_speed_rl(left_speed)
# TODO:This is unsafe! could be going forwards,backwards,left or right
self.state = MotorState.FORWARD
def moveIndependentSpeeds(
self,
front_left,
front_right,
rear_left,
rear_right):
front_left = self.__clipSpeedValue(front_left)
front_right = self.__clipSpeedValue(front_right)
rear_left = self.__clipSpeedValue(rear_left)
rear_right = self.__clipSpeedValue(rear_right)
if self.interface == "I2C":
self.front_left.move(front_left, self.accel)
self.front_right.move(front_right, self.accel)
self.rear_left.move(rear_left, self.accel)
self.rear_right.move(rear_right, self.accel)
elif self.interface == "UDP":
motor_udp.send_motor_speed_fl(front_left)
motor_udp.send_motor_speed_fr(front_right)
motor_udp.send_motor_speed_rl(rear_left)
motor_udp.send_motor_speed_rr(rear_right)
# TODO: This is unsafe! could be going forwards,backwards,left or right
self.state = MotorState.FORWARD
if __name__ == "__main__":
while(True):
key = getKey()
d_logger.debug("Key %s pressed", key)
if(key == 'c'):
motor_udp.stop()
sys.exit(0)
elif(key == 'C'):
motor_udp.stop()
sys.exit(0)
elif(key == 'w'):
motor_udp.send_motor_speed_fl(50)
motor_udp.send_motor_speed_fr(50)
motor_udp.send_motor_speed_rr(50)
motor_udp.send_motor_speed_rl(50)
elif(key == 'W'):
motor_udp.send_motor_speed_fl(100)
motor_udp.send_motor_speed_fr(100)
motor_udp.send_motor_speed_rr(100)
motor_udp.send_motor_speed_rl(100)
elif(key == 'a'):
motor_udp.skid_left(20)
elif(key == 'A'):
motor_udp.skid_left(40)
elif(key == 's'):
motor_udp.send_motor_speed_fl(-50)
motor_udp.send_motor_speed_fr(-50)
motor_udp.send_motor_speed_rr(-50)
motor_udp.send_motor_speed_rl(-50)
