from HexStrDataParser import HexStrDataParser, DataToHexStr
parser = HexStrDataParser('b')
assert(parser.stringToData('0A')[0] == 10)
parser = HexStrDataParser('I')
assert(parser.stringToData('0A000000')[0] == 10)
parser = HexStrDataParser('I')
assert(parser.stringToData('00000080')[0] == 2**31)
parser = HexStrDataParser('I')
assert(parser.stringToData('00010000')[0] == 256)
parser = HexStrDataParser('Ib')
assert(parser.stringToData('000100000A')[0] == 256)
assert(parser.stringToData('000100000A')[1] == 10)
parser = HexStrDataParser('fb')
assert(parser.stringToData('0000803f0A')[0] == 1.0)
assert(parser.stringToData('0000803f0A')[1] == 10)
parser = HexStrDataParser('ff')
assert(parser.stringToData('0000803f000080bf')[0] == 1.0)
assert(parser.stringToData('0000803f000080bf')[1] == -1.0)
packer = DataToHexStr('ffB')
packedStr = packer.dataToString([1.0, -1.0, 255])
parser = HexStrDataParser('ffB')
assert(parser.stringToData(packedStr)[0] == 1.0)
"""
uint32_t time; # 0
uint32_t lineSensors;
uint32_t usedSensors;
uint32_t adc1;
float pidFeedback; # 4
float pidK, pidI, pidD, pidU; # 5 6 7 8
float pwmLeft, pwmRight; # 9 10
float battVoltage; # 11
uint32_t lastLapTime # 12
uint8_t finishLineDetected; # 13
"""
parser = HexStrDataParser("IIII" + "f" + "ffff" + "fff" + "Ib")
"""
float leftPwm, rightPwm; # 0 1
float basePwm, pidK, pidI, pidD; # 2 3 4 5
uint32_t cmdType; # 6
enum CommandFromPC { Stop = 0, Run = 1, SetPid = 2, SetPwm = 3 };
"""
packetBuilder = DataToHexStr("ff" + "ffff" + "I")
packetStruct = [0.2, 0.2, 0.0, 0.0, 0.0, 0.0, 0]
def sendPacketStruct(packetStruct):
global sercom
packet = StartByte + packetBuilder.dataToString(packetStruct) + EndByte
