change controller_chery_sterra_es. not test. need change detection_chassis get velspeed.

src/controller/controller_chery_sterra_es/main.cpp

@@ -76,6 +76,34 @@ std::vector<SignalPackValue> mvectorADSONEBOX1;
 std::vector<SignalPackValue> mvectorADSVCU1;
 
 
+double ADS_1_DrvCtrlReq = 0.0;
+double ADS_1_CtrlReqMod = 0.0;
+double ADS_1_DrvTarTqVld = 0.0;
+double ADS_1_DrvTarTqEnable = 0.0;
+double ADS_1_DrvTarTq = 0.0;
+double ADS_1_TarGearReq = 0.0;
+double ADS_1_TarGearReqVld = 0.0;
+double ADS_1_GearCtrlEnable = 1.0;
+
+double ADS_1_SteerAgReq = 0.0;
+double ADS_1_SteerAgVld = 0.0;
+double ADS_1_SteerPilotAgEna = 0.0;
+
+double gfWheelReq = 0.0;
+double gfTorqueReq = 0.0;
+double gfBrakeReq = 0.0;
+
+double ADS_1_PilotCtrlRepSta = 0.0;
+double ADS_1_PilotParkCtrlType =  0.0;
+double ADS_1_PilotParkBrkDecTar = 0.0;
+double ADS_1_PilotParkCtrlRepMod = 0.0;
+double ADS_1_PilotParkBrkDecTarVld = 0.0;
+double ADS_1_PilotParkBrkDecTarEnable = 0.0;
+double ADS_1_PilotParkDec2StpReq = 0.0;
+double ADS_1_PilotParkDriOffReq = 0.0;
+double ADS_1_ParkCtrlMod = 0.0;
+
+
 
 void set_EPS1_signal(std::string strsigname,double value){
     gpsterraes->set_EPS1_signal(strsigname,value);
@@ -98,42 +126,61 @@ void ExecSend();
 void executeDecition(const iv::brain::decition &decition)
 {
 
+    if(decition.brake()<(-0.0001))
+    {
+        ADS_1_DrvTarTq = 0.0;
+
+        ADS_1_ParkCtrlMod = 1.0;
+        ADS_1_PilotParkDriOffReq = 0.0;
+        ADS_1_PilotParkDec2StpReq = 1.0;
+        ADS_1_PilotParkBrkDecTar = decition.brake();
+        ADS_1_PilotParkBrkDecTarVld = 1.0;
+        ADS_1_PilotParkCtrlType = 1.0;
+        ADS_1_PilotParkBrkDecTarEnable = 1.0;
+    }
+
+    else
+    {
+        if(fabs(gfVehSpd) < 0.1)
+        {
+            ADS_1_PilotParkDriOffReq = 1.0;
+        }
+
+        ADS_1_DrvTarTq = decition.torque();
+        if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
 
+        ADS_1_PilotParkDec2StpReq = 0.0;
+        ADS_1_PilotParkBrkDecTarEnable = 0.0;
+        ADS_1_PilotParkBrkDecTarVld = 0.0;
+        ADS_1_PilotParkBrkDecTar = 0.0;
+
+        ADS_1_SteerAgReq = decition.wheelangle();
+    }
 
 }
 
 void Activate()
 {
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    iv::brain::decition xdecition;
-    xdecition.set_brake(0.0);
-    xdecition.set_torque(0.0);
     //    for(int j=0;j<100000;j++)
     //    {
-    std::cout<<" run "<<std::endl;
+    std::cout<<" activate "<<std::endl;
     for(int i = 0; i < 3; i++){
-        xdecition.set_wheelangle(0.0);
-        xdecition.set_angle_mode(0);
-        xdecition.set_angle_active(0);
-        xdecition.set_acc_active(0);
-        xdecition.set_brake_active(0);
-        //        xdecition.set_brake_type(0);
-        xdecition.set_auto_mode(0);
-        gnState = 0;
-        executeDecition(xdecition);
-        ExecSend();
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    }
-    for(int i = 0; i < 3; i++){
-        xdecition.set_wheelangle(0.0);
-        xdecition.set_angle_mode(1);
-        xdecition.set_angle_active(1);
-        xdecition.set_acc_active(1);
-        xdecition.set_brake_active(1);
-        //        xdecition.set_brake_type(1);
-        xdecition.set_auto_mode(3);
-        gnState = 1;
-        executeDecition(xdecition);
+        ADS_1_DrvCtrlReq = 1.0;
+        ADS_1_CtrlReqMod = 2.0;  //1 Pilot  2 Park
+        ADS_1_DrvTarTqVld = 1.0;
+        ADS_1_DrvTarTqEnable = 1.0;
+        ADS_1_DrvTarTq = 0.0;
+        ADS_1_TarGearReq = 4.0;  //1 P 4 D
+        ADS_1_TarGearReqVld = 1.0;
+        ADS_1_GearCtrlEnable = 0.0;
+
+        ADS_1_SteerAgReq = gfWheelReq;
+        ADS_1_SteerAgVld = 1.0;
+        ADS_1_SteerPilotAgEna = 1.0;
+
+        ADS_1_PilotParkDec2StpReq = 1.0;
+        ADS_1_ParkCtrlMod = 1.0;
+
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
@@ -142,33 +189,43 @@ void Activate()
 
 void UnAcitvate()
 {
-    iv::brain::decition xdecition;
-
-    xdecition.set_brake(0.0);
-    xdecition.set_torque(0.0);
-    for(int i = 0; i < 3; i++){
-        xdecition.set_wheelangle(0);
-        xdecition.set_angle_mode(1);
-        xdecition.set_angle_active(1);
-        xdecition.set_acc_active(1);
-        xdecition.set_brake_active(1);
-        //        xdecition.set_brake_type(1);
-        xdecition.set_auto_mode(3);
-        gnState = 1;
-        executeDecition(xdecition);
-        ExecSend();
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    if(fabs(gfVehSpd)<0.1)
+    {
+        for(int i = 0; i < 3; i++){
+            ADS_1_DrvCtrlReq = 1.0;
+            ADS_1_CtrlReqMod = 2.0;
+            ADS_1_DrvTarTqVld = 1.0;
+            ADS_1_DrvTarTqEnable = 1.0;
+            ADS_1_DrvTarTq = 0.0;
+            ADS_1_TarGearReq = 1.0;
+            ADS_1_TarGearReqVld = 1.0;
+            ADS_1_GearCtrlEnable = 0.0;
+
+            ADS_1_SteerAgReq = 0.0;
+            ADS_1_SteerAgVld = 1.0;
+            ADS_1_SteerPilotAgEna = 1.0;
+
+            ExecSend();
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
     }
     for(int i = 0; i < 3; i++){
-        xdecition.set_wheelangle(0);
-        xdecition.set_angle_mode(0);
-        xdecition.set_angle_active(0);
-        xdecition.set_acc_active(0);
-        xdecition.set_brake_active(0);
-        //        xdecition.set_brake_type(0);
-        gnState = 0;
-        xdecition.set_auto_mode(0);
-        executeDecition(xdecition);
+        ADS_1_DrvCtrlReq = 0.0;
+        ADS_1_CtrlReqMod = 0.0;
+        ADS_1_DrvTarTqVld = 0.0;
+        ADS_1_DrvTarTqEnable = 0.0;
+        ADS_1_DrvTarTq = 0.0;
+        ADS_1_TarGearReq = 1.0;
+        ADS_1_TarGearReqVld = 0.0;
+        ADS_1_GearCtrlEnable = 1.0;
+
+        ADS_1_SteerAgReq = 0.0;
+        ADS_1_SteerAgVld = 0.0;
+        ADS_1_SteerPilotAgEna = 0.0;
+
+        ADS_1_PilotParkDec2StpReq = 0.0;
+        ADS_1_ParkCtrlMod = 0.0;
+
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
@@ -249,8 +306,63 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
 
 }
 
+
+void PrepareMsg()
+{
+    static int rollcouter = 0;
+    //    std::cout<<" roll count:: "<<rollcouter<<std::endl;
+
+    set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
+    set_EPS1_signal("ADS_1_Resd1",0.0);
+    set_EPS1_signal("ADS_1_SteerAgReq",ADS_1_SteerAgReq);
+    set_EPS1_signal("ADS_1_SteerAgVld",ADS_1_SteerAgVld);
+    set_EPS1_signal("ADS_1_SteerPilotAgEna",ADS_1_SteerPilotAgEna);
+    set_EPS1_signal("ADS_1_RollgCntr2",rollcouter);
+    set_EPS1_signal("ADS_1_Resd2",0.0);
+    set_EPS1_signal("ADS_1_SteerTqEna",1.0);
+    set_EPS1_signal("ADS_1_LdwWarningCmd",0.0);
+    set_EPS1_signal("ADS_1_LdwWarningCmdVld",2.0);
+    set_EPS1_signal("ADS_1_SteerMaxTqReq",10.0);
+    set_EPS1_signal("ADS_1_SteerMinTqReq",1.0);
+    set_EPS1_signal("ADS_1_ADSHealthSts",1.0);
+    set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
+    set_EPS1_signal("CutOutMAC_2CB_S",1.0);
+    gpsterraes->GetEPS1Data(ADS_EPS_1);
+
+    set_VCU1_signal("ADS_1_RollgCntr1",rollcouter);
+    set_VCU1_signal("ADS_1_Resd1",0);
+    set_VCU1_signal("ADS_1_DrvTarTq",ADS_1_DrvTarTq);
+    set_VCU1_signal("ADS_1_DrvTarTqVld",ADS_1_DrvTarTqVld);
+    set_VCU1_signal("ADS_1_DrvCtrlReq",ADS_1_DrvCtrlReq);
+    set_VCU1_signal("ADS_1_CtrlReqMod",ADS_1_CtrlReqMod);
+    set_VCU1_signal("ADS_1_DrvTarTqEnable",ADS_1_DrvTarTqEnable);
+    set_VCU1_signal("ADS_1_RollgCntr2",rollcouter);
+    set_VCU1_signal("ADS_1_Resd2",0);
+    set_VCU1_signal("ADS_1_TarGearReq",ADS_1_TarGearReq);
+    set_VCU1_signal("ADS_1_TarGearReqVld",ADS_1_TarGearReqVld);
+    set_VCU1_signal("ADS_1_GearCtrlEnable",ADS_1_GearCtrlEnable);
+    gpsterraes->GetVCU1Data(ADS_VCU_1);
+
+    set_ONEBOX1_signal("ADS_1_RollgCntr1",rollcouter);
+    set_ONEBOX1_signal("ADS_1_Resd1",0);
+    set_ONEBOX1_signal("ADS_1_PilotCtrlRepSta",ADS_1_PilotCtrlRepSta);
+    set_ONEBOX1_signal("ADS_1_PilotParkCtrlType",ADS_1_PilotParkCtrlType);
+    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTar",ADS_1_PilotParkBrkDecTar);
+    set_ONEBOX1_signal("ADS_1_PilotParkCtrlRepMod",ADS_1_PilotParkCtrlRepMod);
+    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarVld",ADS_1_PilotParkBrkDecTarVld);
+    set_ONEBOX1_signal("ADS_1_PilotParkBrkDecTarEnable",ADS_1_PilotParkBrkDecTarEnable);
+    set_ONEBOX1_signal("ADS_1_PilotParkDec2StpReq",ADS_1_PilotParkDec2StpReq);
+    set_ONEBOX1_signal("ADS_1_PilotParkDriOffReq",ADS_1_PilotParkDriOffReq);
+    set_ONEBOX1_signal("ADS_1_ParkCtrlMod",ADS_1_ParkCtrlMod);
+    gpsterraes->GetONEBOX1Data(ADS_ONEBOX_1);
+
+
+    rollcouter++;
+}
+
 void ExecSend()
 {
+    PrepareMsg();
     static int nCount = 0;
     nCount++;
     iv::can::canmsg xmsg;
@@ -284,16 +396,16 @@ void ExecSend()
     xraw.set_bext(false);
     xraw.set_bremote(false);
     xraw.set_len(24);
-    // iv::can::canraw * pxraw159 = xmsg.add_rawmsg();
-    // pxraw159->CopyFrom(xraw);
+    iv::can::canraw * pxraw159 = xmsg.add_rawmsg();
+    pxraw159->CopyFrom(xraw);
 
     xraw.set_id(0x167);
     xraw.set_data(ADS_VCU_1,24);
     xraw.set_bext(false);
     xraw.set_bremote(false);
     xraw.set_len(24);
-    // iv::can::canraw * pxraw167 = xmsg.add_rawmsg();
-    // pxraw167->CopyFrom(xraw);
+    iv::can::canraw * pxraw167 = xmsg.add_rawmsg();
+    pxraw167->CopyFrom(xraw);
 
 
     xmsg.set_channel(0);
@@ -336,6 +448,7 @@ void SendEPS1()
 {
     static int rollcouter = 0;
 //    std::cout<<" roll count:: "<<rollcouter<<std::endl;
+
     set_EPS1_signal("ADS_1_RollgCntr1",rollcouter);
     set_EPS1_signal("ADS_1_Resd1",0.0);
     gpsterraes->GetEPS1Data(ADS_EPS_1);
@@ -544,8 +657,8 @@ int main(int argc, char *argv[])
     EnableTorqueBrakeTest();
 #endif
 
-    testes();
-    return 0;
+    // testes();
+    // return 0;
 
     std::thread xthread(sendthread);
 

src/python/candbc/cansend.py

@@ -0,0 +1,4 @@
+
+def sendcandata(addr,msg,bus):
+	print("can send.")
+	pass

src/python/candbc/cheryes.py

@@ -0,0 +1,221 @@
+from packer import CANPacker
+from parser import CANParser
+
+from cansend import *
+
+import threading  
+import time  
+
+dbcpath = "./ADCC_CH.dbc"
+
+dbcmsgs = [
+      ("ADS_EPS_1", 0),
+      ("ADS_EPS_3", 0),
+	  ("ADS_ONEBOX_1", 0),
+	  ("EPS_3", 0),
+	  ("EPS_4", 0),
+	  ("VCU_2_G", 0),
+	  ("VCU_COM_10", 0),
+	  ("VCU_ADCC_12", 0),
+    ]
+
+fwheel = 90.0
+facc = 0.0
+fmaxwheel = 430
+fminwheel = -430
+fmaxacc = 1.0
+fminacc = -1.0
+fwheelstep = 10.0
+faccstep = 0.1
+
+keyexit = False
+
+ADS_1_SteerPilotAgEna = 0.0  # 2 activate
+ADS_1_SteerAgVld = 0.0
+ADS_1_SteerAgReq = 0.0
+
+ADS_1_RollgCntr1 = 0.0
+
+
+def CRCCheck_SAEJ1850(msg : bytes, len : int, idleCrc : bytes)  -> bytearray:
+	dat = bytearray(msg)
+	poly = bytes([0x1D])
+	crc8 = bytearray(1)
+	crc8[0] = idleCrc[0]
+	for i in range(len) :
+		crc8[0] = crc8[0]^dat[i]
+		for j in range(8) :
+			if (crc8[0] & 0x80):
+				t = (crc8[0] << 1) ^ poly[0]
+				a = t.to_bytes(2, byteorder='little') 
+				crc8[0] = a[0]
+			else:
+				t = crc8[0]<<1
+				a = t.to_bytes(2, byteorder='little') 
+				crc8[0] = a[0]         
+	crc8[0] ^= 0x00
+	return crc8
+
+def  FillCheck_Chery(msg : bytes,iddata : bytes) -> bytearray:
+	dat = bytearray(9)
+	dat[0] = iddata[0]
+	dat[1] = iddata[1]
+	for i in range(1,8):
+		dat[i+1] = msg[i]
+	check = CRCCheck_SAEJ1850(bytes(dat),9,bytes([0x00]))
+	rtn = bytearray(8)
+	rtn[0] = check[0]
+	for i in range(1,8):
+		rtn[i] = msg[i]
+	return rtn
+	
+
+
+
+
+def calccheck(bytes1):
+	check = 80
+	print("check type: ",type(check))
+	x = bytes([check])
+	print(" x type: " ,type(x))
+	after = bytes1[1:]
+	modified_bytes = bytes([check]) + after 
+	return modified_bytes   
+
+def testcheck():
+	#can0  1A5  [48]  4A 0A 00 00 00 00 00 00 19 0A 00 00 7C E1 00 00 A7 0A 00 00 02 00 00 00 51 0A 00 00 7D 00 00 00 50 0A 0F A0 7D 00 00 00 00 00 00 00 00 00 00 00
+	msg1 =bytes([0x00,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0A,0x00,0x00,0x7C,0xE1,0x00,0x00,0x00,0x0A,0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x0A,0x00,0x00,0x7D,0x00,0x00,0x00,0x00,0x0A,0x0F,0xA0,0x7D,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00])
+	iddata = [0x0030,0x0031,0x0032,0x0033,0x0034]
+	for i in range(5):
+		part1 = FillCheck_Chery(msg1[i*8:(i+1)*8],iddata[i].to_bytes(2,byteorder="little"))
+		print("check ",hex(part1[0]))
+
+#testcheck()
+
+
+
+def wheel_task():  
+#	print("Task is running at", time.ctime())
+	global ADS_1_RollgCntr1
+	global packer1
+	global fwheel
+	global ADS_1_SteerAgReq
+	global ADS_1_SteerAgVld
+	addr,msg,bus = packer1.make_can_msg("ADS_EPS_1", 0, {
+	"ADS_1_SteerAgReq":fwheel,
+	"ADS_1_SteerPilotAgEna":ADS_1_SteerPilotAgEna,
+	"ADS_1_SteerAgVld":ADS_1_SteerAgVld,
+	"ADS_1_RollgCntr1":ADS_1_RollgCntr1,
+	})
+	ADS_1_RollgCntr1 = ADS_1_RollgCntr1 + 1.0
+	if(ADS_1_RollgCntr1>14):
+		ADS_1_RollgCntr1 = 0.0
+	iddata = [0x0004,0x0005]
+	part1 = FillCheck_Chery(msg[0:8],iddata[0].to_bytes(2,byteorder="little"))
+	part2 = FillCheck_Chery(msg[8:16],iddata[1].to_bytes(2,byteorder="little"))
+	part3 = bytearray(msg[16:24])
+	msgdata = part1 + part2 + part3
+	print("fwheel: ",fwheel)
+	hex_str = msgdata.hex().upper()  
+	print(hex_str)  
+	sendcandata(addr,bytes(msgdata),bus)
+	if (keyexit == False):
+		threading.Timer(0.01, wheel_task).start()
+
+def drive_task():
+#	print("drive task.")
+	if (keyexit == False):
+		threading.Timer(1.0, drive_task).start()
+
+def brake_task():
+#	print("brake task.");
+	if (keyexit == False):
+		threading.Timer(1.0, brake_task).start()
+
+
+
+
+packer1 = CANPacker(dbcpath)
+
+addr,msg,bus = packer1.make_can_msg("ADS_EPS_1", 0, {
+	"ADS_1_SteerAgReq":fwheel
+	})
+len1 = len(msg)
+print("addr = ",addr)
+print("len =  ",len1)
+print("type = " , type(msg))
+
+wheel_task()
+drive_task()
+brake_task()
+
+
+
+parser1 = CANParser(dbcpath,dbcmsgs,0)
+
+parser1.update_strings([0, [[addr,msg,bus]]])
+
+print("wheel value: ",parser1.vl["ADS_EPS_1"]["ADS_1_SteerAgReq"])
+
+addr,msg,bus = packer1.make_can_msg("ADS_EPS_1", 0, {
+	"ADS_1_SteerAgReq":fwheel+10.0
+	})
+
+parser1.update_strings([0, [[addr,msg,bus]]])
+
+print("wheel value: ",parser1.vl["ADS_EPS_1"]["ADS_1_SteerAgReq"])
+
+print("msg len : ",len(dbcmsgs))
+
+
+try: 
+	while True:
+		userinput = input()
+#	print("input len: ",len(userinput))
+		if(len(userinput)>0):
+			if(userinput[0] == 'a'):
+				fwheel = fwheel + fwheelstep
+				if(fwheel>fmaxwheel):
+					fwheel = fmaxwheel
+				print("wheel: ",fwheel)
+			if(userinput[0] == 'd'):
+				fwheel = fwheel - fwheelstep
+				if(fwheel < fminwheel):
+					fwheel = fminwheel
+				print("wheel: ",fwheel)
+			if(userinput[0] == 'z'):
+				fwheel = 0.0
+				print("wheel: ",fwheel)
+			if(userinput[0] == 'w'):
+				facc = facc + faccstep
+				if(facc > fmaxacc):
+					facc = fmaxacc
+				print("acc = ",facc)
+			if(userinput[0] == 's'):
+				facc = facc - faccstep
+				if(facc < fminacc):
+					facc = fminacc
+				print("acc = ",facc)
+			if(userinput[0] == 'b'):
+				facc = fminacc
+				print("acc = ",facc)
+			if(userinput[0] == ' '):
+				facc = 0.0
+				fwheel = 0.0
+				print("wheel: ",fwheel)
+				print("acc = ",facc)
+			if(userinput[0] == 'i'):
+				ADS_1_SteerPilotAgEna = 2.0  # 2 activate
+				ADS_1_SteerAgVld = 1.0
+			if(userinput[0] == 'j'):
+				ADS_1_SteerPilotAgEna = 0.0  # 2 activate
+				ADS_1_SteerAgVld = 0.0
+
+
+				
+except KeyboardInterrupt:  
+	keyexit = True
+	print("Main thread interrupted and will exit.")  
+
+#msg2 = calccheck(msg)
+#print("data = ",msg2.hex())

src/python/candbc/testpacker.py

@@ -1,6 +1,8 @@
 from packer import CANPacker
 from parser import CANParser
 
+from cansend import *
+
 import threading  
 import time  
 
@@ -90,36 +92,48 @@ def testcheck():
 
 #testcheck()
 
-def sendcandata(addr,msg,bus):
-	pass
+
 
 def wheel_task():  
 #	print("Task is running at", time.ctime())
+	global ADS_1_RollgCntr1
+	global packer1
+	global fwheel
+	global ADS_1_SteerAgReq
+	global ADS_1_SteerAgVld
 	addr,msg,bus = packer1.make_can_msg("ADS_EPS_1", 0, {
 	"ADS_1_SteerAgReq":fwheel,
-	"ADS_1_SteerAgReq":ADS_1_SteerAgReq,
+	"ADS_1_SteerPilotAgEna":ADS_1_SteerPilotAgEna,
 	"ADS_1_SteerAgVld":ADS_1_SteerAgVld,
-	"ADS_1_RollgCntr1":ADS_1_RollgCntr1
+	"ADS_1_RollgCntr1":ADS_1_RollgCntr1,
 	})
 	ADS_1_RollgCntr1 = ADS_1_RollgCntr1 + 1.0
 	if(ADS_1_RollgCntr1>14):
 		ADS_1_RollgCntr1 = 0.0
 	iddata = [0x0004,0x0005]
 	part1 = FillCheck_Chery(msg[0:8],iddata[0].to_bytes(2,byteorder="little"))
-	part2 = FillCheck_Chery(msg[8:16],iddata[0].to_bytes(2,byteorder="little"))
+	part2 = FillCheck_Chery(msg[8:16],iddata[1].to_bytes(2,byteorder="little"))
 	part3 = bytearray(msg[16:24])
 	msgdata = part1 + part2 + part3
+	print("fwheel: ",fwheel)
+	hex_str = msgdata.hex().upper()  
+	print(hex_str)  
 	sendcandata(addr,bytes(msgdata),bus)
 	if (keyexit == False):
 		threading.Timer(0.01, wheel_task).start()
 
 def drive_task():
+#	print("drive task.")
 	if (keyexit == False):
-		threading.Timer(0.01, drive_task).start()
+		threading.Timer(1.0, drive_task).start()
 
 def brake_task():
+#	print("brake task.");
 	if (keyexit == False):
-		threading.Timer(0.01, brake_task).start()
+		threading.Timer(1.0, brake_task).start()
+
+
+
 
 packer1 = CANPacker(dbcpath)
 
@@ -131,6 +145,10 @@ print("addr = ",addr)
 print("len =  ",len1)
 print("type = " , type(msg))
 
+wheel_task()
+drive_task()
+brake_task()
+
 
 
 parser1 = CANParser(dbcpath,dbcmsgs,0)
@@ -155,9 +173,6 @@ try:
 		userinput = input()
 #	print("input len: ",len(userinput))
 		if(len(userinput)>0):
-			if(userinput[0] == 'q'):
-				print("exit ")
-				break
 			if(userinput[0] == 'a'):
 				fwheel = fwheel + fwheelstep
 				if(fwheel>fmaxwheel):
@@ -192,8 +207,9 @@ try:
 			if(userinput[0] == 'i'):
 				ADS_1_SteerPilotAgEna = 2.0  # 2 activate
 				ADS_1_SteerAgVld = 1.0
-			if(userinput[0] == 'o'):
+			if(userinput[0] == 'j'):
 				ADS_1_SteerPilotAgEna = 0.0  # 2 activate
+				ADS_1_SteerAgVld = 0.0