start work with real oliver proto. fix motor driver shim, add very basic motor scaling.

master
Cole Deck 2 years ago
parent 563d82c5f9
commit fc3870c5ba

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<project version="4"> <project version="4">
<component name="CompilerConfiguration"> <component name="CompilerConfiguration">
<bytecodeTargetLevel target="11" /> <bytecodeTargetLevel target="17" />
</component> </component>
</project> </project>

@ -7,6 +7,7 @@
<option name="testRunner" value="GRADLE" /> <option name="testRunner" value="GRADLE" />
<option name="distributionType" value="DEFAULT_WRAPPED" /> <option name="distributionType" value="DEFAULT_WRAPPED" />
<option name="externalProjectPath" value="$PROJECT_DIR$" /> <option name="externalProjectPath" value="$PROJECT_DIR$" />
<option name="gradleJvm" value="Embedded JDK" />
<option name="modules"> <option name="modules">
<set> <set>
<option value="$PROJECT_DIR$" /> <option value="$PROJECT_DIR$" />

@ -29,7 +29,7 @@
</map> </map>
</option> </option>
</component> </component>
<component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="Android Studio default JDK" project-jdk-type="JavaSDK"> <component name="ProjectRootManager" version="2" languageLevel="JDK_17" default="true" project-jdk-name="jbr-17" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/build/classes" /> <output url="file://$PROJECT_DIR$/build/classes" />
</component> </component>
<component name="ProjectType"> <component name="ProjectType">

@ -42,6 +42,7 @@ Implementation Notes
try: try:
from typing import Optional from typing import Optional
import maestro import maestro
import time
#from busio import I2C #from busio import I2C
#from adafruit_motor.servo import Servo, ContinuousServo #from adafruit_motor.servo import Servo, ContinuousServo
except ImportError: except ImportError:
@ -135,6 +136,7 @@ class _Servo:
if servo_channel >= num_channels or servo_channel < 0: if servo_channel >= num_channels or servo_channel < 0:
raise ValueError("servo must be 0-{}!".format(num_channels - 1)) raise ValueError("servo must be 0-{}!".format(num_channels - 1))
servo = pololuservo(servo_channel) servo = pololuservo(servo_channel)
return servo return servo
raise ValueError("Channel {} is already in use.".format(servo_channel)) raise ValueError("Channel {} is already in use.".format(servo_channel))
@ -142,13 +144,19 @@ class _Servo:
return len(self.kit._items) return len(self.kit._items)
class pololuservo: class pololuservo:
def __init__(self, ch, actuation_range: int = 180, min_pulse: int = 3000, max_pulse: int = 9000): def __init__(self, ch, actuation_range: int = 180, min_pulse: int = 1600, max_pulse: int = 9900):
self.ch = ch self.ch = ch
self.actuation_range = actuation_range self.actuation_range = actuation_range
self._min_duty = min_pulse self._min_duty = min_pulse
self._max_duty = max_pulse self._max_duty = max_pulse
self._duty_range = max_pulse = min_pulse self._duty_range = max_pulse - min_pulse
self.duty_cycle = 0 self.duty_cycle = 0
print("init servo", ch)
pca.setAccel(ch, 10)
pca.setTarget(ch, int((max_pulse-min_pulse)/2 + min_pulse)) # set midpoint
time.sleep(0.1)
pca.setAccel(ch, 0)
@property @property
def angle(self) -> Optional[float]: def angle(self) -> Optional[float]:
@ -159,7 +167,7 @@ class pololuservo:
return self.actuation_range * self.fraction return self.actuation_range * self.fraction
@angle.setter @angle.setter
def angle(self, new_angle: Optional[int]) -> None: def angle(self, new_angle: Optional[float]) -> None:
if new_angle is None: # disable the servo by sending 0 signal if new_angle is None: # disable the servo by sending 0 signal
self.fraction = None self.fraction = None
return return
@ -185,7 +193,14 @@ class pololuservo:
return return
if not 0.0 <= value <= 1.0: if not 0.0 <= value <= 1.0:
raise ValueError("Must be 0.0 to 1.0") raise ValueError("Must be 0.0 to 1.0")
#if self.ch > 2 and self.ch < 6:
# valuetmp = 1.0 - value
# self.duty_cycle = self._min_duty + int(valuetmp * self._duty_range)
#else:
self.duty_cycle = self._min_duty + int(value * self._duty_range) self.duty_cycle = self._min_duty + int(value * self._duty_range)
#print(self.ch) #print(self.ch)
pca.setTarget(self.ch,self.duty_cycle) #if self.ch == 0:
#print(value)
pca.setTarget(self.ch,int(self.duty_cycle))
time.sleep(0.002)
#self._pwm_out.duty_cycle = duty_cycle #self._pwm_out.duty_cycle = duty_cycle

@ -38,14 +38,14 @@
"movementInterval": 5, "movementInterval": 5,
"movementSwitchDuration": 150, "movementSwitchDuration": 150,
"leg0Offset": [ "leg0Offset": [
4.0, 0,
6.0, 10,
2 25
], ],
"leg0Scale": [ "leg0Scale": [
1, 1,
1, -1,
1 -1
], ],
"leg1Offset": [ "leg1Offset": [
-1.0, -1.0,
@ -89,8 +89,8 @@
], ],
"leg5Offset": [ "leg5Offset": [
0, 0,
4.0, 10,
-10 -40
], ],
"leg5Scale": [ "leg5Scale": [
1, 1,

@ -93,7 +93,7 @@ class Hexapod(Thread):
self.calibration_mode = False self.calibration_mode = False
with open('/home/cole/Downloads/hexapod/software/raspberry pi/config.json', 'r') as read_file: with open('config.json', 'r') as read_file:
self.config = json.load(read_file) self.config = json.load(read_file)
# legs' coordinates # legs' coordinates
@ -116,40 +116,40 @@ class Hexapod(Thread):
self.mount_position[:, 1] = self.mount_y self.mount_position[:, 1] = self.mount_y
# Objects # Objects
self.pca_right = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0c) self.pca_right = ServoKit(channels=18, address="/dev/ttyACM0", frequency=0x0c)
self.pca_left = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0d) #self.pca_left = ServoKit(channels=18, address="/dev/ttyACM0", frequency=0x0c)
self.pca_left = self.pca_right
self.legs = [ self.legs = [
# front right # front right
Leg(0, Leg(0,
[self.pca_left.servo[15], self.pca_left.servo[14], [self.pca_left.servo[2], self.pca_left.servo[1],
self.pca_left.servo[13]], self.pca_left.servo[0]],
correction=self.config.get('leg0Offset', [0, 0, 0])), correction=self.config.get('leg0Offset', [0, 0, 0]), scale=self.config.get('leg0Scale', [1, 1, 1])),
# center right # center right
Leg(1, Leg(1,
[self.pca_left.servo[8], self.pca_left.servo[4], [self.pca_left.servo[6], self.pca_left.servo[7],
self.pca_left.servo[11]], self.pca_left.servo[8]],
correction=self.config.get('leg1Offset', [0, 0, 0])), correction=self.config.get('leg1Offset', [0, 0, 0]), scale=self.config.get('leg1Scale', [1, 1, 1])),
# rear right # rear right
Leg(2, Leg(2,
[self.pca_left.servo[0], self.pca_left.servo[1], [self.pca_left.servo[12], self.pca_left.servo[13],
self.pca_left.servo[2]], self.pca_left.servo[14]],
correction=self.config.get('leg2Offset', [0, 0, 0])), correction=self.config.get('leg2Offset', [0, 0, 0]), scale=self.config.get('leg2Scale', [1, 1, 1])),
# rear left # rear left
Leg(3, Leg(3,
[self.pca_right.servo[15], self.pca_right.servo[14], [self.pca_right.servo[15], self.pca_right.servo[16],
self.pca_right.servo[13]], self.pca_right.servo[17]],
correction=self.config.get('leg3Offset', [0, 0, 0])), correction=self.config.get('leg3Offset', [0, 0, 0]), scale=self.config.get('leg3Scale', [1, 1, 1])),
# center left # center left
Leg(4, Leg(4,
[self.pca_right.servo[7], self.pca_right.servo[11], [self.pca_right.servo[9], self.pca_right.servo[10],
self.pca_right.servo[6]], self.pca_right.servo[11]],
correction=self.config.get('leg4Offset', [0, 0, 0])), correction=self.config.get('leg4Offset', [0, 0, 0]), scale=self.config.get('leg4Scale', [1, 1, 1])),
# front left # front left
Leg(5, Leg(5,
[self.pca_right.servo[0], self.pca_right.servo[2], [self.pca_right.servo[5], self.pca_right.servo[4],
self.pca_right.servo[5]], self.pca_right.servo[3]],
correction=self.config.get('leg5Offset', [0, 0, 0]))] correction=self.config.get('leg5Offset', [0, 0, 0]), scale=self.config.get('leg5Scale', [1, 1, 1]))]
# self.leg_0.reset(True) # self.leg_0.reset(True)
# self.leg_1.reset(True) # self.leg_1.reset(True)
@ -263,7 +263,7 @@ class Hexapod(Thread):
try: try:
cmd_string = self.cmd_queue.get(block=False) cmd_string = self.cmd_queue.get(block=False)
print('interrput') print('interrupt')
except Empty: except Empty:
# time.sleep(self.interval) # time.sleep(self.interval)
pass pass
@ -300,7 +300,6 @@ class Hexapod(Thread):
angles[:, 1] = 90-((ar + a1) * 180 / np.pi) angles[:, 1] = 90-((ar + a1) * 180 / np.pi)
angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90 angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90
return angles return angles
def cmd_handler(self, cmd_string): def cmd_handler(self, cmd_string):
@ -352,7 +351,7 @@ class Hexapod(Thread):
def save_config(self): def save_config(self):
try: try:
json.dump(self.config, open( json.dump(self.config, open(
'/home/pi/hexapod/software/raspberry pi/config.json', 'w+'), indent=4) '/home/amelia/hexapod/software/raspberry pi/config2.json', 'w+'), indent=4)
except PermissionError as err: except PermissionError as err:
pass pass
@ -362,7 +361,7 @@ class Hexapod(Thread):
try: try:
cmd_string = self.cmd_queue.get(block=False) cmd_string = self.cmd_queue.get(block=False)
except Empty: except Empty:
# time.sleep(self.interval) #time.sleep(self.interval)
pass pass
else: else:
self.cmd_handler(cmd_string) self.cmd_handler(cmd_string)

@ -38,6 +38,7 @@ class Leg:
self.junction_servos = junction_servos self.junction_servos = junction_servos
self.correction = correction self.correction = correction
self.constraint = constraint self.constraint = constraint
self.scale = scale
def set_angle(self, junction, angle): def set_angle(self, junction, angle):
set_angle = np.min( set_angle = np.min(
@ -50,8 +51,19 @@ class Leg:
self.junction_servos[junction].angle = angle self.junction_servos[junction].angle = angle
def move_junctions(self, angles): def move_junctions(self, angles):
if self.scale[0] < 0:
self.set_angle(0, self.constraint[0][1] + self.constraint[0][0] - angles[0])
#print(self.scale, self.constraint[0][1] + self.constraint[0][0] - angles[0])
else:
self.set_angle(0, angles[0]) self.set_angle(0, angles[0])
#print(self.scale, angles[0])
if self.scale[1] < 0:
self.set_angle(1, self.constraint[1][1] + self.constraint[1][0] - angles[1])
else:
self.set_angle(1, angles[1]) self.set_angle(1, angles[1])
if self.scale[2] < 0:
self.set_angle(2, self.constraint[2][1] + self.constraint[2][0] - angles[2])
else:
self.set_angle(2, angles[2]) self.set_angle(2, angles[2])
def reset(self, calibrated=False): def reset(self, calibrated=False):

Loading…
Cancel
Save