diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 9434eec3ee..6f5df7155e 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -3,21 +3,25 @@ import numpy as np from common.realtime import DT_CTRL from selfdrive.car.body import bodycan from opendbc.can.packer import CANPacker +from selfdrive.car.body.values import SPEED_FROM_RPM MAX_TORQUE = 500 MAX_TORQUE_RATE = 50 MAX_ANGLE_ERROR = 7 +MAX_POS_INTEGRATOR = 0.2 # meters +MAX_TURN_INTEGRATOR = 0.1 # meters + class CarController(): def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) self.i_speed = 0 + self.i_speed_diff = 0 self.i_balance = 0 self.d_balance = 0 - self.i_torque = 0 self.speed_measured = 0. self.speed_desired = 0. @@ -48,49 +52,50 @@ class CarController(): # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) # //// + # Setpoint speed PID - kp_speed = 0.001 - ki_speed = 0.00001 + kp_speed = 0.001 / SPEED_FROM_RPM + ki_speed = 0.001 / SPEED_FROM_RPM alpha_speed = 1.0 - self.speed_measured = (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. - self.speed_desired = (1. - alpha_speed)*self.speed_desired - p_speed = (self.speed_desired - self.speed_measured) - self.i_speed += ki_speed * p_speed - self.i_speed = np.clip(self.i_speed, -0.1, 0.1) - set_point = p_speed * kp_speed + self.i_speed + self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. + self.speed_desired = (1. - alpha_speed) * self.speed_desired + speed_error = self.speed_desired - self.speed_measured + self.i_speed += speed_error * DT_CTRL + self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR) + set_point = kp_speed * speed_error + ki_speed * self.i_speed # Balancing PID kp_balance = 1300 ki_balance = 0 kd_balance = 280 - alpha_d_balance = 1.0 # Clip angle error, this is enough to get up from stands p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR)) self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr - self.d_balance = np.clip(((1. - alpha_d_balance) * self.d_balance + alpha_d_balance * -CC.angularVelocity[1]), -1., 1.) + self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.) torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000)) - # Positional recovery PID - kp_torque = 0.95 - ki_torque = 0.1 + # yaw recovery PID + kp_turn = 0.1 / SPEED_FROM_RPM + ki_turn = 0.1 / SPEED_FROM_RPM - p_torque = (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) - self.i_torque += (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) - torque_diff = int(np.clip(p_torque*kp_torque + self.i_torque*ki_torque, -100, 100)) + speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) + self.i_speed_diff += speed_diff_measured * DT_CTRL + self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR) + torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100)) # Combine 2 PIDs outputs torque_r = torque + torque_diff torque_l = torque - torque_diff # Torque rate limits - self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10) , + self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), self.torque_r_filtered - MAX_TORQUE_RATE, - self.torque_r_filtered + MAX_TORQUE_RATE) + self.torque_r_filtered + MAX_TORQUE_RATE) self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), self.torque_l_filtered - MAX_TORQUE_RATE, - self.torque_l_filtered + MAX_TORQUE_RATE) + self.torque_l_filtered + MAX_TORQUE_RATE) torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index 9f0ab93fbb..2ef93aa98c 100644 --- a/selfdrive/car/body/carstate.py +++ b/selfdrive/car/body/carstate.py @@ -3,6 +3,7 @@ from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.body.values import DBC +STARTUP_TICKS = 100 class CarState(CarStateBase): def update(self, cp): diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 54639b40c6..8f36e1ed0d 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -2,6 +2,7 @@ from cereal import car from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod @@ -20,7 +21,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 9 ret.wheelbase = 0.406 - ret.wheelSpeedFactor = 0.008587 + ret.wheelSpeedFactor = SPEED_FROM_RPM ret.centerToFront = ret.wheelbase * 0.44 ret.radarOffCan = True diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index c6b0f92137..11b4e5e8bd 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -5,6 +5,7 @@ from selfdrive.car import dbc_dict from selfdrive.car.docs_definitions import CarInfo Ecu = car.CarParams.Ecu +SPEED_FROM_RPM = 0.008587 class CarControllerParams: ANGLE_DELTA_BP = [0., 5., 15.] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7729326d06..efc74e2102 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -abe47ced1758bb6017e61ccf4e8cf19d0b71819e \ No newline at end of file +f7a6dd296e4747595ec4ef453c58800a88128be7 \ No newline at end of file