import time
import serial


class Pololu1376:
	def __init__(self, serial_port):
		self.ser = serial_port
		self.ser.set_output_flow_control(False)
		try:
			self.ser.open()
		except serial.SerialException:
			self.ser.close()
			self.ser.open()

		print("F/W-Version: ", self.get_firmware_version())

	def cmd_send(self, cmd):
		self.ser.write((cmd + '\r\n').encode())

	def cmd_recv(self):
		data = self.ser.readline().decode("utf-8").replace("\n", '').replace("\r", '')
		return data

	def go(self):
		self.cmd_send("GO")

	def motor_forward(self):
		self.cmd_send("F")

	def motor_reverse(self):
		self.cmd_send("R")

	def motor_brake(self):
		self.cmd_send("B")

	def motor_stop(self):
		self.cmd_send("X")

	def motor_set_limit(self, value):
		self.cmd_send("L" + str(value))

	def get_firmware_version(self):
		self.cmd_send("V")
		return self.cmd_recv()

	def get_variable(self, var):
		self.cmd_send("L" + str(var))
		return self.cmd_recv()


if __name__ == '__main__':
	ser = serial.Serial("/dev/ttyACM0", 115200)

	s = Pololu1376(serial_port=ser)

	print("End of program")