From f177c427080f425d61d4858dd4b4c973324cf858 Mon Sep 17 00:00:00 2001 From: sherlockholmes1603 Date: Wed, 19 May 2021 12:16:09 +0530 Subject: [PATCH 1/3] yaw control --- scripts/__pycache__/bot.cpython-38.pyc | Bin 0 -> 2825 bytes scripts/bot.py | 5 +- scripts/initial.py | 40 ++++++++++++++ scripts/movement.py | 2 +- scripts/position.py | 70 +++++++++++++++++++++++++ scripts/trial.py | 59 +++++++++++++++------ 6 files changed, 158 insertions(+), 18 deletions(-) create mode 100644 scripts/__pycache__/bot.cpython-38.pyc create mode 100644 scripts/initial.py create mode 100644 scripts/position.py diff --git a/scripts/__pycache__/bot.cpython-38.pyc b/scripts/__pycache__/bot.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bc9391598880428a9c66cd2b9b3e4d9a0ae0763d GIT binary patch literal 2825 zcma)8OOM>f5oR|z9M0^lwAKr@gTzb@4j9O$QiekV`bagjbtg5d1s=D9X z-0WIthw-Ps{{piwX|djHEUuwfmqCOjSY{2F8To)?Lte8XtNADV&lcX;$~mR6B`ny-8|{Is+vRSSCb`nA)LvFZ;=OnF zu4|qOy>XE4g>jK9S!8-^J_*yjQc)f!fuo%&$%fjV%1pa3y(}u(fiVv(c@D$ZS9iw6 zB-x3_(KtHV*-NS;Rg^m=ZS+O3H(Q36UVJl>;!Y%!ohp`Tsj40FLwT&d5ZY1|hJ=~Q z0EcC4yFEk><)D7SdPa#D}H6DtslQj}uuXFZk)ukNh%{ zDvhUEBz-tbCH}{eOrzUb;zMG;$P@ZE_vbG4qij_8chW5L(@|bX`Wh)es)(V2XeiuX zT1ANNsz{C!6~g0zCka~eBFMm-zrVYj9WQ4yy)plaaAvx<5Rs~QycBpG-7&Fi$a%=r z-eS*NSXIW)J&!%j-FXNWpJPJvq7meY)r8`~!Z%PPno@b87xM zK&x5^f_=(Q9B{gG&bHvs(4U^_sIEZhPC2Ff24H-+VI;NgyNozif4JY~579=bz1jZK zsV3BjjZCT%K2IrBYaup~pfoU{lBpU`{ArcuBmdyWp1+jxfa$a4__#{rs>!Jv!sNK5 zyifo2=+UFV)@K^W-%7F~PSvq)K~J2>?||i@DF{JN64do~5#iUODmf^sRHa4!>s;(h z%G8KfvIicMZu$}!U!P`)ye^B$@20?;%!%U+Nx6vlyhV`xZJ`Re+hp^PwR>lrBw3}m zZoU5Y{_c&N?}xkl{hROXzpd>_bQdJewTIbtGA?KtjAqQSz!6upl)331~t$ zXgXoa`J**N@fP+ykFa#g_N-G-#M$Y~XYtPy5-BJzqZ{*T>%(UuWiGcS=_sl9?OPVJ zKEkm;rNM~l2Tl{79=vWGdm~2#m(d8g=G!mn)*<=)0c(T^cjp}^ZWzc76x>ZbDZSn#(=Zi+!b(gcj#Ql8Qu{b0q)98{2JI_{o%sj-}vWDU6nt> zT2dYBKZYJkQ|hL=kF{8=VHB&FH zduYkgT%Ee|Rd7nmAo)`wk#DU=3^hW-D^*WTqOS*}nd&DlxF?y)nk{1>_Oi5vg` literal 0 HcmV?d00001 diff --git a/scripts/bot.py b/scripts/bot.py index be523f4..822d4aa 100644 --- a/scripts/bot.py +++ b/scripts/bot.py @@ -29,12 +29,13 @@ def setpoint(self, self.target_pos = ((target_x, target_y, target_z), (target_roll, target_pitch, target_yaw)) else: self.target_pos = target_pos + return target_pos def control(self): ''' This is responsible for the bots control algorithm using PID ''' pos, orie = p.getBasePositionAndOrientation(self.id, physicsClientId = self.pClient) - euler = p.getEulerFromQuaternion(orie) + euler = p.getEulerFromQuaternion(orie) feedback = control_instance(pos, euler) p.setJointMotorControl2(self.id, self.wheels[0], @@ -42,7 +43,7 @@ def control(self): targetVelocity = -max(min(15,feedback),-15), force = self.maxForce, physicsClientId = self.pClient) - p.setJointMotorControl2(self.id, + p.setJointMotorControl2(self.id, self.wheels[1], p.VELOCITY_CONTROL, targetVelocity= -max(min(15,feedback),-15), diff --git a/scripts/initial.py b/scripts/initial.py new file mode 100644 index 0000000..d2c2b88 --- /dev/null +++ b/scripts/initial.py @@ -0,0 +1,40 @@ +import pybullet as p +import pybullet_data +from time import sleep, time +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.loadURDF("plane.urdf") +botpos=[0,0,0.1] +bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +p.setGravity(0,0,-10) +numJoints = p.getNumJoints(bot) +for joint in range(numJoints): + print(p.getJointInfo(bot,joint)) +wheels = [ 2, 5 ] +targetVel = 15 +maxForce = 6 +kp, kd, ki = 0.05, 0.005, 0.001 +init = time() +target_pos = 0 +prev_error = None +encoder_pos = [0,0] +while(1): + orie = p.getBasePositionAndOrientation(bot)[1] + euler = p.getEulerFromQuaternion(orie) + pitch = euler[1] + dt = time()-init + error = (pitch-target_pos) + if prev_error is None: + prev_error = error + feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) + print("feedback: ",feedback) + print("error: ", error) + feedback/=500 + encoder_pos[0]-=feedback + encoder_pos[1]-=feedback + p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + p.stepSimulation() + sleep(0.05) + init = time() \ No newline at end of file diff --git a/scripts/movement.py b/scripts/movement.py index edde5d4..5578054 100644 --- a/scripts/movement.py +++ b/scripts/movement.py @@ -16,7 +16,7 @@ target_pos = 0 prev_error = None while(1): - orie = p.getBasePositionAndOrientation(bot)[1] + orie = p.getBasePositionAndOrientation(bot[1]) euler = p.getEulerFromQuaternion(orie) pitch = euler[1] dt = time()-init diff --git a/scripts/position.py b/scripts/position.py new file mode 100644 index 0000000..3adada3 --- /dev/null +++ b/scripts/position.py @@ -0,0 +1,70 @@ +import pybullet as p +import pybullet_data +from time import sleep, time +import numpy as np + +def magnitude(a): + return (a[0]**2+a[1]**2)**0.5 + +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.loadURDF("plane.urdf") +botpos=[0,0,0.1] +bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +p.setGravity(0,0,-10) +numJoints = p.getNumJoints(bot) +for joint in range(numJoints): + print(p.getJointInfo(bot,joint)) +wheels = [ 2, 5 ] +targetVel = 15 +maxForce = 6 +kp, kd, ki = 0.005, 0.005, 0.000 +init = time() +target_x = 0 +target_pitch =0 +prev_error = 0 +prev_error_pitch = 0 +encoder_pos = [0,0] +encoder_vel = [0,0] +while(1): + posi = p.getBasePositionAndOrientation(bot)[0] + orie = p.getBasePositionAndOrientation(bot)[1] + euler = p.getEulerFromQuaternion(orie) + pitch = euler[1] + dt = time()-init + p1 = p.getLinkState(bot,9)[0] + p2 = p.getLinkState(bot,10)[0] + p3 = p.getLinkState(bot,11)[0] + p1 = np.array(p1) + p2 = np.array(p2) + p3 = np.array(p3) + direc = p1- (p2+p3)/2 + sum = direc + posi + sum = magnitude(sum) + direc = magnitude(direc) + posi = magnitude(posi) + error = sum - direc -posi + feedback = kp*error + kd*(error - prev_error)/dt + feedback/=10 + error_pitch = (pitch-target_pitch) + feedback1 = kp*error_pitch + kd*(error_pitch - prev_error_pitch)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) + prev_error_pitch = error_pitch + feedback1/=500 + if (error_pitch<0.01 and error_pitch>-0.01): + encoder_vel[0] = -feedback + encoder_vel[1] = feedback + print("direction: ", feedback, error) + p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + else: + encoder_pos[0]-=feedback1 + encoder_pos[1]-=feedback1 + print("pitch: ", feedback1, error_pitch) + p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + prev_error = error + p.stepSimulation() + sleep(0.05) + init = time() + diff --git a/scripts/trial.py b/scripts/trial.py index 366cac7..d84f75f 100644 --- a/scripts/trial.py +++ b/scripts/trial.py @@ -1,6 +1,7 @@ import pybullet as p import pybullet_data from time import sleep, time +from bot import Bot p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") @@ -10,30 +11,58 @@ numJoints = p.getNumJoints(bot) for joint in range(numJoints): print(p.getJointInfo(bot,joint)) +#car = Bot() wheels = [ 2, 5 ] targetVel = 15 maxForce = 6 -kp, kd, ki = 0.005, 0.005, 0.000 +kp, kd, ki = 0.05, 0.005, 0.000 init = time() -target_pos = 0 -prev_error = None +#target_pos = car.setpoint(target_roll = 0, target_yaw = 0, target_pitch =0) +target_pitch = 0 +target_yaw = 0 +target_roll = 0 +prev_error_pitch = None +prev_error_yaw = None +prev_error_roll = None encoder_pos = [0,0] +encoder_vel = [0,0] +i=0 +'''Bhayiya isme agar error in pitch +kam hai toh yaw 0 ho raha hai now i am +only working on pitch so that uska +error kam ho sake''' while(1): orie = p.getBasePositionAndOrientation(bot)[1] euler = p.getEulerFromQuaternion(orie) pitch = euler[1] + yaw = euler[2] + roll = euler[0] dt = time()-init - error = (pitch-target_pos) - if prev_error is None: - prev_error = error - feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) - #print(feedback, pitch) - feedback/=500 - encoder_pos[0]-=feedback - encoder_pos[1]-=feedback - p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) - p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) - #print(list(p.getJointState(bot, wheel) for wheel in wheels)) - p.stepSimulation() + error = (pitch-target_pitch) + if prev_error_pitch is None: + prev_error_pitch = error + feedback1 = kp*error + kd*(error - prev_error_pitch)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) + feedback1/=500 + error1 = yaw-target_yaw + if prev_error_yaw is None: + prev_error_yaw = error1 + feedback2 = kp*error1 + kd*(error1 - prev_error_yaw)/dt + feedback2/=100 + if(error<0.1 and error>-0.1): + encoder_vel[0] = -feedback2 + encoder_vel[1] = feedback2 + print("yaw: ",error1, pitch) + p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), + p.stepSimulation() + else: + encoder_pos[0] -= feedback1 + encoder_pos[1] -= feedback1 + print("pitch: ", error, yaw) + p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + p.stepSimulation() + i+=1 sleep(0.05) init = time() From 942763780ef5a9c71eacf7368830d2d4a313e8c2 Mon Sep 17 00:00:00 2001 From: sherlockholmes1603 Date: Mon, 24 May 2021 00:02:02 +0530 Subject: [PATCH 2/3] real values of pid control --- scripts/lqr.py | 68 ++++++++++++++++++++++++++++++++++++++++++++++++ scripts/pitch.py | 49 ++++++++++++++++++++++++++++++++++ 2 files changed, 117 insertions(+) create mode 100644 scripts/lqr.py create mode 100644 scripts/pitch.py diff --git a/scripts/lqr.py b/scripts/lqr.py new file mode 100644 index 0000000..39b17d5 --- /dev/null +++ b/scripts/lqr.py @@ -0,0 +1,68 @@ +import pybullet as p +import pybullet_data +from time import sleep, time +from control import lqr +import numpy as np +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.loadURDF("plane.urdf") +botpos=[0,0,0.1] +bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +p.setGravity(0,0,-10) +numJoints = p.getNumJoints(bot) +for joint in range(numJoints): + print(p.getJointInfo(bot,joint)) +wheels = [ 2, 5 ] +targetVel = 15 +maxForce = 6 +A = [[38.7316,12.879,5.6979,9.9588], + [-104.7609,39.2818,18.0856,30.5764], + [83.2656,-30.5537,-13.2043,-23.5357], + [-44.2548,16.7696,6.3255,12.654]] + +B = [[-0.7188], + [-2.0030], + [1.5985], + [-0.8379]] + +Q = [[100,0,0,0], + [0,1,0,0], + [0,0,100,0], + [0,0,0,1]] +R = 0.5 +kp, kd, ki = 0.005, 0.005, 0.000 +init = time() +target_pos = 0 +prev_error = 0 +inti_term = 0 +encoder_pos = [0,0] +while(1): + orie = p.getBasePositionAndOrientation(bot)[1] + euler = p.getEulerFromQuaternion(orie) + pitch = euler[1] + dt = time()-init + error = (pitch-target_pos) + # if prev_error is None: + # prev_error = error + # feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) + k = lqr(A,B,Q,R)[0] + print("k: ") + k = np.resize(k,(4,1)) + print(k) + if abs(error)<0.01: + inti_term += error*dt + else: + inti_term = 0 + feedback = -k[0] * error - k[1]*(error - prev_error) - k[2]*inti_term + feedback/=500 + print("feedback: ", feedback) + print("error: ", error) + prev_error = error + encoder_pos[0]-=feedback + encoder_pos[1]-=feedback + p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + p.stepSimulation() + sleep(0.05) + init = time() \ No newline at end of file diff --git a/scripts/pitch.py b/scripts/pitch.py new file mode 100644 index 0000000..45146a9 --- /dev/null +++ b/scripts/pitch.py @@ -0,0 +1,49 @@ +import pybullet as p +import pybullet_data +from time import sleep, time +# import control +# import slycot +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.loadURDF("plane.urdf") +botpos=[0,0,0.1] +bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +p.setGravity(0,0,-10) +numJoints = p.getNumJoints(bot) +for joint in range(numJoints): + print(p.getJointInfo(bot,joint)) +wheels = [ 2, 5 ] +targetVel = 15 +maxForce = 6 +kp, kd, ki = 255,26,34 +init = time() +target_pos = 0 +prev_error = 0 +inti_term = 0 +encoder_pos = [0,0] +while(1): + orie = p.getBasePositionAndOrientation(bot)[1] + euler = p.getEulerFromQuaternion(orie) + pitch = euler[1] + dt = time()-init + error = (pitch-target_pos) + #k = control.lqr(A,B,Q,R) + if abs(error)<0.01: + inti_term += error*dt + else: + inti_term = 0 + feedback = kp*error + kd*(error - prev_error)/dt + ki*inti_term + prev_error = error + print("error: ", error) + feedback/=500 + #feedback = - k * error + print("feedback: ",feedback) + encoder_pos[0] -= feedback + encoder_pos[1] -= feedback + p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + p.stepSimulation() + sleep(0.05) + init = time() + From 2fb8c9f24f98015d59467c6e4c20e2e29b602c90 Mon Sep 17 00:00:00 2001 From: sherlockholmes1603 Date: Tue, 25 May 2021 19:11:52 +0530 Subject: [PATCH 3/3] trial.py mein yaw control --- scripts/initial.py | 2 +- scripts/lqr.py | 8 +++-- scripts/pitch.py | 9 +++-- scripts/trial.py | 82 ++++++++++++++++++++++++---------------------- 4 files changed, 55 insertions(+), 46 deletions(-) diff --git a/scripts/initial.py b/scripts/initial.py index d2c2b88..dd2e04c 100644 --- a/scripts/initial.py +++ b/scripts/initial.py @@ -13,7 +13,7 @@ wheels = [ 2, 5 ] targetVel = 15 maxForce = 6 -kp, kd, ki = 0.05, 0.005, 0.001 +kp, kd, ki = 0.005, 0.005, 0.000 init = time() target_pos = 0 prev_error = None diff --git a/scripts/lqr.py b/scripts/lqr.py index 39b17d5..194c89a 100644 --- a/scripts/lqr.py +++ b/scripts/lqr.py @@ -45,10 +45,14 @@ # if prev_error is None: # prev_error = error # feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) - k = lqr(A,B,Q,R)[0] + k , s , e = lqr(A,B,Q,R) print("k: ") - k = np.resize(k,(4,1)) + # k = np.resize(k,(4,1)) print(k) + print(" s :") + print(s) + print(" e :") + print(e) if abs(error)<0.01: inti_term += error*dt else: diff --git a/scripts/pitch.py b/scripts/pitch.py index 45146a9..dba8f9e 100644 --- a/scripts/pitch.py +++ b/scripts/pitch.py @@ -6,8 +6,11 @@ p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") -botpos=[0,0,0.1] -bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +botpos=[0,0,0.08] +botori = p.getQuaternionFromEuler([0, 0, 0]) + +bot = p.loadURDF("urdf/Paucibot.urdf", *botpos, *botori) +# bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) p.setGravity(0,0,-10) numJoints = p.getNumJoints(bot) for joint in range(numJoints): @@ -17,7 +20,7 @@ maxForce = 6 kp, kd, ki = 255,26,34 init = time() -target_pos = 0 +target_pos = 0.0 prev_error = 0 inti_term = 0 encoder_pos = [0,0] diff --git a/scripts/trial.py b/scripts/trial.py index d84f75f..ffed5b9 100644 --- a/scripts/trial.py +++ b/scripts/trial.py @@ -1,68 +1,70 @@ import pybullet as p import pybullet_data from time import sleep, time -from bot import Bot +# import control +# import slycot p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") -botpos=[0,0,0.1] -bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) +botpos=[0,0,0.08] +botori = p.getQuaternionFromEuler([0, 0, 0]) + +bot = p.loadURDF("urdf/Paucibot.urdf", *botpos, *botori) +# bot = p.loadURDF("urdf/Paucibot.urdf",*botpos) p.setGravity(0,0,-10) numJoints = p.getNumJoints(bot) for joint in range(numJoints): print(p.getJointInfo(bot,joint)) -#car = Bot() wheels = [ 2, 5 ] targetVel = 15 maxForce = 6 -kp, kd, ki = 0.05, 0.005, 0.000 +kp, kd, ki = 255,26,34 init = time() -#target_pos = car.setpoint(target_roll = 0, target_yaw = 0, target_pitch =0) -target_pitch = 0 +target_pitch = 0.0 target_yaw = 0 -target_roll = 0 -prev_error_pitch = None -prev_error_yaw = None -prev_error_roll = None +prev_error = 0 +prev_error1 = 0 +inti_term = 0 +inti_term1 = 0 encoder_pos = [0,0] encoder_vel = [0,0] -i=0 -'''Bhayiya isme agar error in pitch -kam hai toh yaw 0 ho raha hai now i am -only working on pitch so that uska -error kam ho sake''' while(1): orie = p.getBasePositionAndOrientation(bot)[1] euler = p.getEulerFromQuaternion(orie) pitch = euler[1] yaw = euler[2] - roll = euler[0] dt = time()-init error = (pitch-target_pitch) - if prev_error_pitch is None: - prev_error_pitch = error - feedback1 = kp*error + kd*(error - prev_error_pitch)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0) - feedback1/=500 - error1 = yaw-target_yaw - if prev_error_yaw is None: - prev_error_yaw = error1 - feedback2 = kp*error1 + kd*(error1 - prev_error_yaw)/dt - feedback2/=100 - if(error<0.1 and error>-0.1): - encoder_vel[0] = -feedback2 - encoder_vel[1] = feedback2 - print("yaw: ",error1, pitch) - p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) - p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), + error1 = yaw -target_yaw + #k = control.lqr(A,B,Q,R) + if abs(error)<0.01: + inti_term += error*dt + if abs(error1)<0.01: + inti_term1 += error1*dt + else: + inti_term1 = 0 + feedback1 = 1*error1 + 1*(error1 - prev_error1)/dt + 1*inti_term1 + prev_error1 = error1 + print("Yaw control - error:", error1," feedback:", feedback1) + encoder_vel[0] = -feedback1 + encoder_vel[1] = feedback1 + p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce) + p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce) p.stepSimulation() else: - encoder_pos[0] -= feedback1 - encoder_pos[1] -= feedback1 - print("pitch: ", error, yaw) - p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) - p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) - #print(list(p.getJointState(bot, wheel) for wheel in wheels)) - p.stepSimulation() - i+=1 + inti_term = 0 + feedback = 255*error + 26*(error - prev_error)/dt + 34*inti_term + prev_error = error + print("error: ", error) + feedback/=500 + #feedback = - k * error + print("feedback: ",feedback) + encoder_pos[0] -= feedback + encoder_pos[1] -= feedback + p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),) + p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), ) + #print(list(p.getJointState(bot, wheel) for wheel in wheels)) + p.stepSimulation() sleep(0.05) init = time() +