Skip to content

Commit

Permalink
Simulator started
Browse files Browse the repository at this point in the history
  • Loading branch information
@kubasaw
kubasaw committed Dec 10, 2019
1 parent 3547a4f commit df6b307
Show file tree
Hide file tree
Showing 3 changed files with 78 additions and 39 deletions.
56 changes: 40 additions & 16 deletions car/canListener.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
from struct import unpack

import serial
from PyQt5.QtCore import QObject, pyqtSignal
from PyQt5.QtCore import *


# class canFrameAppender(Listener, QObject):
Expand Down Expand Up @@ -34,10 +34,10 @@

class myCan(QObject):

appendFrameToLog = pyqtSignal(str)
setThrottle = pyqtSignal(float)
strSignal = pyqtSignal(str)
tupleSignal = pyqtSignal(tuple)

def __init__(self, serialPortName):
def __init__(self, serialPortName, strSlots=[], tupleSlots=[], loopTime=1):
super().__init__()
self.__serialhandle = serial.Serial(serialPortName, 115200)
self.__serialhandle.reset_input_buffer()
Expand All @@ -55,14 +55,26 @@ def __init__(self, serialPortName):
if chars != b'\r':
raise OSError('Bad char!')

for slot in strSlots:
self.strSignal.connect(slot)

for slot in tupleSlots:
self.tupleSignal.connect(slot)

self.__messageBuffer = bytes()

self.__receiveTimer = QTimer(self)
self.__receiveTimer.setInterval(loopTime*1000)
self.__receiveTimer.timeout.connect(self.__readSerial)
self.__receiveTimer.start()

def __del__(self):
self.__receiveTimer.stop()
self.__serialhandle.write(b'C\r')
self.__serialhandle.close()

def readSerial(self):
print("LEN: "+str(len(self.__messageBuffer)))
def __readSerial(self):
# print("LEN: "+str(len(self.__messageBuffer)))

while self.__serialhandle.in_waiting:
self.__messageBuffer = self.__messageBuffer+self.__serialhandle.read()
Expand All @@ -71,27 +83,34 @@ def readSerial(self):

def __parseForMsg(self):
while(1):
(frame, sep, after)=self.__messageBuffer.partition(b'\r')
if not sep:
(frame, sep, after) = self.__messageBuffer.partition(b'\r')
self.__messageBuffer = after
if (not sep):
break
elif (len(frame) < 5):
continue
else:
frame=frame.decode()
id = int(frame[1:4],16)
frame = frame.decode()
id = int(frame[1:4], 16)
dlc = int(frame[4])
#print(dlc)
data=[]
# print(dlc)
data = []
for i in range(dlc):
data.append(int(frame[5 + i * 2:7 + i * 2], 16))
messageReceived=canMsg(id,data)
messageReceived = canMsg(id, data)
print(messageReceived)
self.__messageBuffer=after
self.strSignal.emit(str(messageReceived))
if messageReceived.stdId == 19:
engine = unpack(
'fb', bytes(messageReceived.data[0:5]))
self.tupleSignal.emit(engine)

def sendMsg(self, msg):
self.__serialhandle.write(b't')
# print(msg.stdId)
# print('{:03x}'.format(msg.stdId))
self.__serialhandle.write('{:03X}'.format(msg.stdId).encode())
self.__serialhandle.write('{:d}'.format(len(msg.data)).encode())
self.__serialhandle.write('{:d}'.format(msg.dlc).encode())
for sign in msg.data:
self.__serialhandle.write('{:02X}'.format(sign).encode())
self.__serialhandle.write(b'\r')
Expand All @@ -102,6 +121,11 @@ class canMsg():
def __init__(self, StdId, Data):
self.stdId = StdId
self.data = Data
self.dlc = len(Data)

def __str__(self):
return "ID: {ID:#x}\t Data: {Data}".format(Data=[hex(no) for no in self.data], ID=self.stdId)
if self.stdId == 19:
return "ID: {ID:#x}\t Data: {Data}".format(Data=unpack(
'fb', bytes(self.data[0:5])), ID=self.stdId)
else:
return "ID: {ID:#x}\t Data: {Data}".format(Data=[hex(no) for no in self.data], ID=self.stdId)
23 changes: 16 additions & 7 deletions car/motion.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ def __init__(self, initialCondition=[0., 0., 0.]):
self.__state = np.array(initialCondition)
self.__time = 0
self.__throttle = 0
self.__nextSwitch = 0
self.__switchingPoints = {0: 0.0}
self.param = constants()

# Simulation control
Expand Down Expand Up @@ -187,8 +187,15 @@ def getThrottle(self):
"""
return self.__throttle

def setNextSwitchingPoint(self, time):
self.__nextSwitch = time
def setSwitchingPoint(self, tup):
self.__switchingPoints[tup[1]] = tup[0]

def getNextSwitchingPoint(self):
for i in sorted(self.__switchingPoints.keys()):
if self.__switchingPoints[i] > self.__time:
return self.__switchingPoints[i]

return float("inf")

def setThrottle(self, throttle):
"""Set actual throttle value for next simulation time steps
Expand Down Expand Up @@ -282,16 +289,18 @@ def carDynamics(t, x, throttle):
t0 = self.__time
tf = self.__time+self.__timestep

if(t0 < self.__nextSwitch and tf > self.__nextSwitch):
self.__state = solve_ivp(lambda t, x: carDynamics(t, x, self.__throttle), (t0, self.__nextSwitch),
self.__state, t_eval=[self.__nextSwitch])['y'].flatten()
__nextSwitch = self.getNextSwitchingPoint()

if(t0 < __nextSwitch and tf > __nextSwitch):
self.__state = solve_ivp(lambda t, x: carDynamics(t, x, self.__throttle), (t0, __nextSwitch),
self.__state, t_eval=[__nextSwitch])['y'].flatten()
if (self.__throttle <= 0):
self.__state[2] += self.param.get("r0")
self.setThrottle(1)
else:
self.setThrottle(0)

self.__state = solve_ivp(lambda t, x: carDynamics(t, x, self.__throttle), (self.__nextSwitch, tf),
self.__state = solve_ivp(lambda t, x: carDynamics(t, x, self.__throttle), (__nextSwitch, tf),
self.__state, t_eval=[tf])['y'].flatten()

else:
Expand Down
38 changes: 22 additions & 16 deletions main.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
import sys

import serial.tools.list_ports
import can
#import can

import gui
import car
Expand Down Expand Up @@ -50,14 +50,19 @@ def on_makeStepButton_clicked(self):
try:
# self.__car.setThrottle(self.engineField.text())
self.__car.makeStep()
motionMessage=can.Message(arbitration_id=18,is_extended_id=False,data=self.__car.getCanBytes()[:])
# motionMessage=can.Message(arbitration_id=18,is_extended_id=False,data=self.__car.getCanBytes()[:])
motionMessage = car.canMsg(
StdId=18, Data=self.__car.getCanBytes()[:])
print(motionMessage)
self.__canbus.send(motionMessage)
self.__canbus.sendMsg(motionMessage)
except Exception as e:
QtWidgets.QMessageBox.critical(
self, _translate("Dialog", "Error"), str(e))

self.populateFields()
if float(self.timeField.text())>=240:
if self.simulationStart.isChecked()==True:
self.simulationStart.click()

def populateFields(self):
self.timeField.setText(f"{self.__car.getSimTime():.2f}")
Expand Down Expand Up @@ -103,7 +108,9 @@ def on_refreshAvailablePorts_clicked(self):
def on_connectPort_clicked(self, state):
if state:
try:
self.__canbus = car.myCan(self.availablePorts.currentData())
self.__canbus = car.myCan(self.availablePorts.currentData(), [
self.canReceived.appendPlainText], [
self.__car.setSwitchingPoint], loopTime=0.1)
except Exception as e:
self.connectPort.setChecked(False)
QtWidgets.QMessageBox.critical(
Expand All @@ -115,35 +122,34 @@ def on_connectPort_clicked(self, state):
self.availablePorts.setEnabled(False)
self.refreshAvailablePorts.setEnabled(False)


else:
self.connectPort.setText(_translate("MainWindow", "Connect"))
del(self.__canbus)
self.canInterfaceTypes.setEnabled(True)
self.availablePorts.setEnabled(True)
self.refreshAvailablePorts.setEnabled(True)

@pyqtSlot(bool)
def on_simulationStart_clicked(self,state):
def on_simulationStart_clicked(self, state):
if state:
self.__ticker = QTimer(self)
self.__ticker.setInterval(100)
self.__ticker.timeout.connect(self.on_makeStepButton_clicked)
self.__ticker.start()
self.__car.setThrottle(1)
self.simulationStart.setText(_translate("MainWindow","Stop Simulation!"))
lapData=1
self.simulationStart.setText(
_translate("MainWindow", "Stop Simulation!"))
lapData = 1

else:
self.__ticker.stop()
self.simulationStart.setText(_translate("MainWindow","Start Simulation!"))
lapData=0

lapMessage=can.Message(arbitration_id=32,is_extended_id=False,data=[lapData])
print(lapMessage)
self.__canbus.send(lapMessage)

self.simulationStart.setText(
_translate("MainWindow", "Start Simulation!"))
lapData = 0

lapMessage = car.canMsg(StdId=32, Data=[lapData])
print(lapMessage)
self.__canbus.sendMsg(lapMessage)


if __name__ == "__main__":
Expand Down

0 comments on commit df6b307

Please sign in to comment.