<repo_name>kjetilfjellheim/rov<file_sep>/lm298.py
import logging

"""
Constants
"""
LOGGER = "lm298"
"""
Logger setup
"""
logger = logging.getLogger("lm298")

"""
Class definitions
"""
class MotorControl:
    """
    Constants
    """
    DIRECTION_LEFT = 1
    DIRECTION_RIGHT = -1
    DIRECTION_FORWARD = 1
    DIRECTION_BACK = -1
    STOP_DUTY_CYCLE = 0
    HIGH = 1
    LOW = 0

    """
    Class variables
    """
    pwm1Pin = None    #Pwm pin for motor 1
    m1Pin = None      #Motor pin for motor 1
    pwm2Pin = None    #Pwm pin for motor 2
    m2Pin = None      #Motor pin for motor 2

    def __init__(self, pwm1Pin, m1Pin, m2Pin, pwm2Pin):
        logger.info("Starting initalizing LM298")
        self.pwm1Pin = pwm1Pin
        self.m1Pin = m1Pin
        self.m2Pin = m2Pin
        self.pwm2Pin = pwm2Pin
        self.stop()
        logger.info("Finished initalizing LM298")

    def setForward(self, mPin):
        mPin.write(self.HIGH)
        if mPin == self.m1Pin:
            logger.info("Motor 1 written value HIGH")
        elif mPin == self.m2Pin:
            logger.info("Motor 2 written value HIGH")

    def setReverse(self, mPin):
        mPin.write(self.LOW)
        if mPin == self.m1Pin:
            logger.info("Motor 1 written value LOW")
        elif mPin == self.m2Pin:
            logger.info("Motor 2 written value LOW")        

    """
    Setting pwm value on motor. The value is a fraction between 0-1.
    """        
    def setPwm(self, pwm, value):
        writeVal = value / 100.0
        pwm.write(writeVal)
        if pwm == self.pwm1Pin:
            logger.info("PWM motor 1 written value {0} dutycycle set {1}".format(writeVal, value))
        elif pwm == self.pwm2Pin:
            logger.info("PWM motor 2 written value {0} dutycycle set {1}".format(writeVal, value))               

    """
    Rotate in place by setting one motor forward and he other back at the same speed.
    """  
    def rotateInPlace(self, speed, direction):
        if self.DIRECTION_LEFT == direction:
            self.setReverse(self.m1Pin)
            self.setForward(self.m2Pin)
        else:
            self.setForward(self.m1Pin)
            self.setReverse(self.m2Pin)        
        logger.info("Motors rotate in place")
        self.setPwm(self.pwm1Pin, 100 - speed)
        self.setPwm(self.pwm2Pin, speed)

    """
    Turn by setting left and right motors at different speed.
    """  
    def turnDifferential(self, speedLeft, speedRight, direction):
        logger.info("Motors turn differential")
        if self.DIRECTION_FORWARD == direction:
            self.setForward(self.m1Pin)
            self.setForward(self.m2Pin)
        else:
            self.setReverse(self.m1Pin)
            self.setReverse(self.m2Pin)
        self.setPwm(self.pwm1Pin, 100 - speedLeft)
        self.setPwm(self.pwm2Pin, speedRight)

    """
    Move forward or back
    """  
    def forward(self, speed, direction):
        logger.info("Motors forward/reverse")
        if self.DIRECTION_FORWARD == direction:
            self.setForward(self.m1Pin)
            self.setForward(self.m2Pin)            
        else:
            self.setReverse(self.m1Pin)
            self.setReverse(self.m2Pin)
        self.setPwm(self.pwm1Pin, 100 - speed)            
        self.setPwm(self.pwm2Pin, speed)
    
    """
    Stop both motors by disbaling enable pins and pwm to stop.
    """  
    def stop(self):
        logger.info("Motors stop")
        self.setReverse(self.m1Pin)
        self.setReverse(self.m2Pin)        
        self.setPwm(self.pwm1Pin, self.STOP_DUTY_CYCLE)
        self.setPwm(self.pwm2Pin, self.STOP_DUTY_CYCLE)<file_sep>/main.py
import threading
import logging
import logging.config
import yaml
from pyfirmata import Arduino
from sen0386 import Sen0386
from lm298 import MotorControl

"""
Constants
"""
LOGGER = "main"                 #Logger name
SEN0386_SERIALNO = "AB0O5A7Z"   #Serial no of th ftdi interface device
MOTORCONTROL_PWM1 = "d:6:p"     #Digital pin 6 pwm output
MOTORCONTROL_M1 = "d:5:o"       #Digital pin 5 output
MOTORCONTROL_M2 = "d:4:o"       #Digital pin 4 output
MOTORCONTROL_PWM2 = "d:3:p"     #Digital pin 3 pwm output
ARDUINO_DEVICE = "/dev/ttyACM0" #Arduino device on Lattepanda
"""
Current sensor values
"""
sen038SensorValues = None       #Latest sen0386 sensor values read

"""
Setup logging    
"""
with open(file = 'logging.yaml', mode = 'r') as stream:
    config = yaml.load(stream = stream, Loader = yaml.FullLoader)
    logging.config.dictConfig(config)

logger = logging.getLogger(LOGGER)
logger.info("Logging setup")
"""
Setting up modules
"""
logger.info("Starting setup")
sen0386 = Sen0386(serialno = SEN0386_SERIALNO)
logger.info("Sen0386 setup")

"""
Setting up gpio
"""
logger.info("Starting setup gpio")
board = Arduino(ARDUINO_DEVICE)
pwm1Pin = board.get_pin(MOTORCONTROL_PWM1)
m1Pin = board.get_pin(MOTORCONTROL_M1)
m2Pin = board.get_pin(MOTORCONTROL_M2)
pwm2Pin = board.get_pin(MOTORCONTROL_PWM2)
logger.info("Finished setup gpio")

"""
Setting up motorcontrol
"""
motorControl = MotorControl(pwm1Pin, m1Pin, m2Pin, pwm2Pin)

"""
Thread module setup
"""
def sen038Thread():
    logger.info("Starting sen038Thread")  
    while True:
        sen038SensorValues = sen0386.readSensorValues()

logger.info("Starting thread setup")        
sen0386Thread = threading.Thread(target = sen038Thread)
sen0386Thread.start()
logger.info("Completed Sen0386 setup")

"""Just demo"""
#motorControl.forward(speed = 100, direction = MotorControl.DIRECTION_FORWARD)			--No movement
#motorControl.forward(speed = 100, direction = MotorControl.DIRECTION_BACK)			    --OK
#motorControl.rotateInPlace(speed = 100, direction = MotorControl.DIRECTION_LEFT)		--OK
#motorControl.rotateInPlace(speed = 100, direction = MotorControl.DIRECTION_RIGHT)		--No movement
#motorControl.turnDifferential(100,0, MotorControl.DIRECTION_FORWARD)					--No movement
#motorControl.turnDifferential(0,100, MotorControl.DIRECTION_FORWARD)					--No movement
#motorControl.turnDifferential(100,0, MotorControl.DIRECTION_BACK)						--OK
#motorControl.turnDifferential(0,100, MotorControl.DIRECTION_BACK)						--OK, except left stil slightly move
#motorControl.stop()																	--OK
<file_sep>/sen0386.py
import struct
import serial
import serial.tools.list_ports as ports
import logging

"""
Logger setup
"""
logger = logging.getLogger("sen0386")       #Calculated sensor logger
loggerHw = logging.getLogger("sen0386_hw")  #Actual hardware values

"""
Class definitions
"""
class Acceleration:
    def __init__(self, ax, ay, az):
        self.ax = ax
        self.ay = ay
        self.az = az
    
    
class AngularVelocity:
    def __init__(self, wx, wy, wz):
        self.wx = wx
        self.wy = wy
        self.wz = wz
    
class Gyro:
    def __init__(self, roll, pitch, yaw):
        self.roll = roll
        self.pitch = pitch
        self.yaw = yaw

class Response:
    def __init__(self, acceleration, angularVelocity, gyro):
        self.acceleration = acceleration
        self.angularVelocity = angularVelocity
        self.gyro = gyro
    

class Sen0386:

    PACKET_HEADER = 0x55
    ACC_PACKET_HEADER = 0x51
    ANGVEL_PACKET_HEADER = 0x52
    ANG_PACKET = 0x53
    ACCPACKET_BUFFER_START = 0
    ACCPACKET_BUFFER_END = 10
    ANGVELPACKET_BUFFER_START = 11
    ANGVELPACKET_BUFFER_END = 21
    ANGPACKET_BUFFER_START = 22
    ANGPACKET_BUFFER_END = 32
    BUFFER_SIZE = 100
    DATA_SIZE = 44
    PACKET_LENGTH = 10
    BYTE_PACKET_HEADER = 0
    BYTE_PACKET_TYPE = 1
    BYTE_AXL_DATA_CONTENT = 2
    BYTE_AXH_DATA_CONTENT = 3
    BYTE_AYL_DATA_CONTENT = 4
    BYTE_AYH_DATA_CONTENT = 5
    BYTE_AZL_DATA_CONTENT = 6
    BYTE_AZH_DATA_CONTENT = 7
    BYTE_WXL_DATA_CONTENT = 2
    BYTE_WXH_DATA_CONTENT = 3
    BYTE_WYL_DATA_CONTENT = 4
    BYTE_WYH_DATA_CONTENT = 5
    BYTE_WZL_DATA_CONTENT = 6
    BYTE_WZH_DATA_CONTENT = 7
    BYTE_ROLLL_DATA_CONTENT = 2
    BYTE_ROLLH_DATA_CONTENT = 3
    BYTE_PITCHL_DATA_CONTENT = 4
    BYTE_PITCHH_DATA_CONTENT = 5
    BYTE_YAWL_DATA_CONTENT = 6
    BYTE_YAWH_DATA_CONTENT = 7
    MAX_16_BIT_INTEGER_SIZE = 32768.00
    GRAVITY_CONSTANT = 9.81
    CALCULATION_GRAVITY_CONSTANT = GRAVITY_CONSTANT * 16.00
    CALCULATION_ROTATION_CONSTANT = 2000.00
    CALCULATION_ANGLE_CONSTANT = 180.00

    def __init__(self, vid=None, pid=None, serialno=None):
        usb_port = self.findUsbPort(vid, pid, serialno)
        self.port = serial.Serial(
                        port=usb_port.device,
                        baudrate=9600,
                        parity=serial.PARITY_NONE,
                        stopbits=serial.STOPBITS_ONE,
                        bytesize=serial.EIGHTBITS,
                        timeout=1)
        self.port.dtr = False
        self.port.rts = False
            
    def findUsbPort(self, vid, pid, serialno):
        com_ports = list(ports.comports())
        for port in com_ports:
            if  (vid == None or port.vid == vid) and (pid == None or port.pid == pid) and (serialno == None or port.serial_number == serialno):
                return port
        return None

    def convertShort(self, packet, highByte, lowByte):
        b = bytearray(2)
        b[0] = packet[highByte]
        b[1] = packet[lowByte]
        result = struct.unpack(">h", b)[0]
        return result

    def handleAccPacket(self, accPacketData):
        if len(accPacketData) == self.PACKET_LENGTH and accPacketData[self.BYTE_PACKET_HEADER] is self.PACKET_HEADER and accPacketData[self.BYTE_PACKET_TYPE] is self.ACC_PACKET_HEADER:
            ax = (self.CALCULATION_GRAVITY_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(accPacketData, self.BYTE_AXH_DATA_CONTENT, self.BYTE_AXL_DATA_CONTENT)
            ay = (self.CALCULATION_GRAVITY_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(accPacketData, self.BYTE_AYH_DATA_CONTENT, self.BYTE_AYL_DATA_CONTENT)
            az = (self.CALCULATION_GRAVITY_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(accPacketData, self.BYTE_AZH_DATA_CONTENT, self.BYTE_AZL_DATA_CONTENT)
            return Acceleration(ax, ay, az)
        else:
            return Acceleration(None, None, None)

    def handleAngVelPacket(self, angVelPacketData):
        if len(angVelPacketData) == self.PACKET_LENGTH and angVelPacketData[self.BYTE_PACKET_HEADER] is self.PACKET_HEADER and angVelPacketData[self.BYTE_PACKET_TYPE] is self.ANGVEL_PACKET_HEADER:
            wx = (self.CALCULATION_ROTATION_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angVelPacketData, self.BYTE_WXH_DATA_CONTENT, self.BYTE_WXL_DATA_CONTENT)
            wy = (self.CALCULATION_ROTATION_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angVelPacketData, self.BYTE_WYH_DATA_CONTENT, self.BYTE_WYL_DATA_CONTENT)
            wz = (self.CALCULATION_ROTATION_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angVelPacketData, self.BYTE_WZH_DATA_CONTENT, self.BYTE_WZL_DATA_CONTENT)
            return AngularVelocity(wx, wy, wz)
        else:
            return AngularVelocity(None, None, None)

    def handleAngPacket(self, angPacketData):
        if len(angPacketData) == self.PACKET_LENGTH and angPacketData[self.BYTE_PACKET_HEADER] is self.PACKET_HEADER and angPacketData[self.BYTE_PACKET_TYPE] is self.ANG_PACKET:
            roll = (self.CALCULATION_ANGLE_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angPacketData, self.BYTE_ROLLH_DATA_CONTENT, self.BYTE_ROLLL_DATA_CONTENT)
            pitch = (self.CALCULATION_ANGLE_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angPacketData, self.BYTE_PITCHH_DATA_CONTENT, self.BYTE_PITCHL_DATA_CONTENT)
            yaw = (self.CALCULATION_ANGLE_CONSTANT / self.MAX_16_BIT_INTEGER_SIZE) * self.convertShort(angPacketData, self.BYTE_YAWH_DATA_CONTENT, self.BYTE_YAWL_DATA_CONTENT)
            return Gyro(roll, pitch, yaw)
        else:
            return Gyro(None, None, None)

    def handlePackets(self, accPacketData, angVelPacketData, angPacketData):
        acceleration = self.handleAccPacket(accPacketData)
        angularVelocity = self.handleAngVelPacket(angVelPacketData)
        gyro = self.handleAngPacket(angPacketData)
        return Response(acceleration, angularVelocity, gyro)
    
    def findStartIndex(self, data):
        for index in range(self.BUFFER_SIZE):
            if data[index] == self.PACKET_HEADER and data[index+1] == self.ACC_PACKET_HEADER:
                return index
        return None

    def readSensorValues(self):
        self.port.flush()
        data = self.port.read(self.BUFFER_SIZE)
        if data is not None and len(data) == self.BUFFER_SIZE:
            first = self.findStartIndex(data)
            if (first != None):
                data = data[first:first+self.DATA_SIZE]
                loggerHw.info(data)
                accPacket = data[self.ACCPACKET_BUFFER_START:self.ACCPACKET_BUFFER_END]
                angVelPacket = data[self.ANGVELPACKET_BUFFER_START:self.ANGVELPACKET_BUFFER_END]
                angPacket = data[self.ANGPACKET_BUFFER_START:self.ANGPACKET_BUFFER_END]
                response = self.handlePackets(accPacket, angVelPacket, angPacket)
                logger.info("Sensor values read ax={0},ay={1},az={2},wx={3},wy={4},wz={5},roll={6},pitch={7},yaw={8}".format(response.acceleration.ax, response.acceleration.ay, response.acceleration.az, response.angularVelocity.wx, response.angularVelocity.wy, response.angularVelocity.wz, response.gyro.roll, response.gyro.pitch, response.gyro.yaw))
                return response
            else:
                logger.info("No sensor values read")
                return Response(None, None, None)
        else:
            logger.info("No sensor values read")
            return Response(None, None, None)