patch out half of the shaking issue

unofficial-rev-port · Jun 20, 2024 · 3547512 · 3547512
1 parent a1c73ba
commit 3547512
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/REVHubInterface/REVMotor.py b/REVHubInterface/REVMotor.py
@@ -183,6 +183,8 @@ def __init__(self, commObj, channel, destinationModule):
         self.destinationModule = destinationModule
         self.commObj = commObj
         self.motorCurrent = REVADC.ADCPin(self.commObj, 8 + channel, self.destinationModule)
+        self.targetPositon = None
+        self.targetVeolcity = None
 
     def setDestination(self, destinationModule):
         self.destinationModule = destinationModule
@@ -222,7 +224,8 @@ def resetEncoder(self):
         resetMotorEncoder(self.commObj, self.destinationModule, self.channel)
 
     def setPower(self, powerLevel):
-        setMotorConstantPower(self.commObj, self.destinationModule, self.channel, powerLevel)
+        if self.targetPositon and self.targetVelocity is None:
+            setMotorConstantPower(self.commObj, self.destinationModule, self.channel, powerLevel)
 
     def getPower(self):
         return getMotorConstantPower(self.commObj, self.destinationModule, self.channel)
@@ -240,6 +243,7 @@ def getTargetVelocity(self):
         return getMotorTargetVelocity(self.commObj, self.destinationModule, self.channel)
 
     def setTargetPosition(self, position, tolerance):
+        self.targetPositon = position
         setMotorTargetPosition(self.commObj, self.destinationModule, self.channel, position, tolerance)
 
     def getTargetPosition(self):
@@ -286,6 +290,9 @@ def getPositionPID(self):
 
     def getBulkPIDData(self):
         return getBulkPIDData(self.commObj, self.destinationModule, self.channel)
+    def disablePIDs(self):
+        self.targetPositon = None
+        self.targetVeolcity = None
 
     def init(self):
         self.setMode(0, 1)

diff --git a/REVHubInterface/__main__.py b/REVHubInterface/__main__.py
@@ -726,9 +726,11 @@ def javaMotorEntry(self, motorNumber, moduleNumber, *args):
         return True
 
     def javaTargetEntry(self, motorNumber, moduleNumber, *args):
+        self.speedMotorEntry(motorNumber, moduleNumber)
         target = int(self.pid_packs[moduleNumber * 4 + motorNumber].Java_entry.get())
         self.REVModules[moduleNumber].motors[motorNumber].setTargetPosition(target, 1)
         self.REVModules[moduleNumber].motors[motorNumber].setMode(2, 2)
+        self.REVModules[moduleNumber].motors[motorNumber].setTargetVelocity(32000)
         self.REVModules[moduleNumber].motors[motorNumber].enable()
 
         self.repetitiveFunctions = [(lambda: self.send_all_KA())]