From fc3870c5bafed908946a86bf5e6258c680a3e621 Mon Sep 17 00:00:00 2001
From: Cole Deck <cole@deck.sh>
Date: Mon, 8 May 2023 00:22:33 -0500
Subject: [PATCH] start work with real oliver proto. fix motor driver shim, add
 very basic motor scaling.

---
 software/android/.idea/compiler.xml        |  2 +-
 software/android/.idea/gradle.xml          |  1 +
 software/android/.idea/misc.xml            |  2 +-
 software/raspberry pi/adafruit_servokit.py | 23 ++++++++--
 software/raspberry pi/config.json          | 14 +++---
 software/raspberry pi/hexapod.py           | 51 +++++++++++-----------
 software/raspberry pi/leg.py               | 18 ++++++--
 7 files changed, 69 insertions(+), 42 deletions(-)

diff --git a/software/android/.idea/compiler.xml b/software/android/.idea/compiler.xml
index fb7f4a8..b589d56 100644
--- a/software/android/.idea/compiler.xml
+++ b/software/android/.idea/compiler.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="CompilerConfiguration">
-    <bytecodeTargetLevel target="11" />
+    <bytecodeTargetLevel target="17" />
   </component>
 </project>
\ No newline at end of file
diff --git a/software/android/.idea/gradle.xml b/software/android/.idea/gradle.xml
index a2d7c21..a0de2a1 100644
--- a/software/android/.idea/gradle.xml
+++ b/software/android/.idea/gradle.xml
@@ -7,6 +7,7 @@
         <option name="testRunner" value="GRADLE" />
         <option name="distributionType" value="DEFAULT_WRAPPED" />
         <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="gradleJvm" value="Embedded JDK" />
         <option name="modules">
           <set>
             <option value="$PROJECT_DIR$" />
diff --git a/software/android/.idea/misc.xml b/software/android/.idea/misc.xml
index b1d4dfc..e44dd90 100644
--- a/software/android/.idea/misc.xml
+++ b/software/android/.idea/misc.xml
@@ -29,7 +29,7 @@
       </map>
     </option>
   </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" />
   </component>
   <component name="ProjectType">
diff --git a/software/raspberry pi/adafruit_servokit.py b/software/raspberry pi/adafruit_servokit.py
index 5e48399..531d3a6 100644
--- a/software/raspberry pi/adafruit_servokit.py	
+++ b/software/raspberry pi/adafruit_servokit.py	
@@ -42,6 +42,7 @@ Implementation Notes
 try:
     from typing import Optional
     import maestro
+    import time
     #from busio import I2C
     #from adafruit_motor.servo import Servo, ContinuousServo
 except ImportError:
@@ -135,6 +136,7 @@ class _Servo:
         if servo_channel >= num_channels or servo_channel < 0:
             raise ValueError("servo must be 0-{}!".format(num_channels - 1))
         servo = pololuservo(servo_channel)
+        
         return servo
         raise ValueError("Channel {} is already in use.".format(servo_channel))
 
@@ -142,13 +144,19 @@ class _Servo:
         return len(self.kit._items)
 
 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.actuation_range = actuation_range
         self._min_duty = min_pulse
         self._max_duty = max_pulse
-        self._duty_range = max_pulse = min_pulse
+        self._duty_range = max_pulse - min_pulse
         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
     def angle(self) -> Optional[float]:
@@ -159,7 +167,7 @@ class pololuservo:
         return self.actuation_range * self.fraction
     
     @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
             self.fraction = None
             return
@@ -185,7 +193,14 @@ class pololuservo:
             return
         if not 0.0 <= value <= 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)
         #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
\ No newline at end of file
diff --git a/software/raspberry pi/config.json b/software/raspberry pi/config.json
index 8ccb714..7654bb3 100644
--- a/software/raspberry pi/config.json	
+++ b/software/raspberry pi/config.json	
@@ -38,14 +38,14 @@
     "movementInterval": 5,
     "movementSwitchDuration": 150,
     "leg0Offset": [
-        4.0,
-        6.0,
-        2
+        0,
+        10,
+        25
     ],
     "leg0Scale": [
         1,
-        1,
-        1
+        -1,
+        -1
     ],
     "leg1Offset": [
         -1.0,
@@ -89,8 +89,8 @@
     ],
     "leg5Offset": [
         0,
-        4.0,
-        -10
+        10,
+        -40
     ],
     "leg5Scale": [
         1,
diff --git a/software/raspberry pi/hexapod.py b/software/raspberry pi/hexapod.py
index 1c46a09..fdb74d4 100644
--- a/software/raspberry pi/hexapod.py	
+++ b/software/raspberry pi/hexapod.py	
@@ -93,7 +93,7 @@ class Hexapod(Thread):
 
         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)
 
         # legs' coordinates
@@ -116,40 +116,40 @@ class Hexapod(Thread):
         self.mount_position[:, 1] = self.mount_y
 
         # Objects
-        self.pca_right = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0c)
-        self.pca_left = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0d)
-
+        self.pca_right = ServoKit(channels=18, address="/dev/ttyACM0", frequency=0x0c)
+        #self.pca_left = ServoKit(channels=18, address="/dev/ttyACM0", frequency=0x0c)
+        self.pca_left = self.pca_right
         self.legs = [
             # front right
             Leg(0,
-                [self.pca_left.servo[15], self.pca_left.servo[14],
-                 self.pca_left.servo[13]],
-                correction=self.config.get('leg0Offset', [0, 0, 0])),
+                [self.pca_left.servo[2], self.pca_left.servo[1],
+                 self.pca_left.servo[0]],
+                correction=self.config.get('leg0Offset', [0, 0, 0]), scale=self.config.get('leg0Scale', [1, 1, 1])),
             # center right
             Leg(1,
-                [self.pca_left.servo[8], self.pca_left.servo[4],
-                 self.pca_left.servo[11]],
-                correction=self.config.get('leg1Offset', [0, 0, 0])),
+                [self.pca_left.servo[6], self.pca_left.servo[7],
+                 self.pca_left.servo[8]],
+                correction=self.config.get('leg1Offset', [0, 0, 0]), scale=self.config.get('leg1Scale', [1, 1, 1])),
             # rear right
             Leg(2,
-                [self.pca_left.servo[0], self.pca_left.servo[1],
-                 self.pca_left.servo[2]],
-                correction=self.config.get('leg2Offset', [0, 0, 0])),
+                [self.pca_left.servo[12], self.pca_left.servo[13],
+                 self.pca_left.servo[14]],
+                correction=self.config.get('leg2Offset', [0, 0, 0]), scale=self.config.get('leg2Scale', [1, 1, 1])),
             # rear left
             Leg(3,
-                [self.pca_right.servo[15], self.pca_right.servo[14],
-                 self.pca_right.servo[13]],
-                correction=self.config.get('leg3Offset', [0, 0, 0])),
+                [self.pca_right.servo[15], self.pca_right.servo[16],
+                 self.pca_right.servo[17]],
+                correction=self.config.get('leg3Offset', [0, 0, 0]), scale=self.config.get('leg3Scale', [1, 1, 1])),
             # center left
             Leg(4,
-                [self.pca_right.servo[7], self.pca_right.servo[11],
-                 self.pca_right.servo[6]],
-                correction=self.config.get('leg4Offset', [0, 0, 0])),
+                [self.pca_right.servo[9], self.pca_right.servo[10],
+                 self.pca_right.servo[11]],
+                correction=self.config.get('leg4Offset', [0, 0, 0]), scale=self.config.get('leg4Scale', [1, 1, 1])),
             # front left
             Leg(5,
-                [self.pca_right.servo[0], self.pca_right.servo[2],
-                 self.pca_right.servo[5]],
-                correction=self.config.get('leg5Offset', [0, 0, 0]))]
+                [self.pca_right.servo[5], self.pca_right.servo[4],
+                 self.pca_right.servo[3]],
+                correction=self.config.get('leg5Offset', [0, 0, 0]), scale=self.config.get('leg5Scale', [1, 1, 1]))]
 
         # self.leg_0.reset(True)
         # self.leg_1.reset(True)
@@ -263,7 +263,7 @@ class Hexapod(Thread):
 
             try:
                 cmd_string = self.cmd_queue.get(block=False)
-                print('interrput')
+                print('interrupt')
             except Empty:
                 # time.sleep(self.interval)
                 pass
@@ -300,7 +300,6 @@ class Hexapod(Thread):
 
         angles[:, 1] = 90-((ar + a1) * 180 / np.pi)
         angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90
-
         return angles
 
     def cmd_handler(self, cmd_string):
@@ -352,7 +351,7 @@ class Hexapod(Thread):
     def save_config(self):
         try:
             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:
             pass
 
@@ -362,7 +361,7 @@ class Hexapod(Thread):
             try:
                 cmd_string = self.cmd_queue.get(block=False)
             except Empty:
-                # time.sleep(self.interval)
+                #time.sleep(self.interval)
                 pass
             else:
                 self.cmd_handler(cmd_string)
diff --git a/software/raspberry pi/leg.py b/software/raspberry pi/leg.py
index 6e6cee6..6d5f82a 100644
--- a/software/raspberry pi/leg.py	
+++ b/software/raspberry pi/leg.py	
@@ -38,6 +38,7 @@ class Leg:
         self.junction_servos = junction_servos
         self.correction = correction
         self.constraint = constraint
+        self.scale = scale
 
     def set_angle(self, junction, angle):
         set_angle = np.min(
@@ -50,9 +51,20 @@ class Leg:
         self.junction_servos[junction].angle = angle
 
     def move_junctions(self, angles):
-        self.set_angle(0, angles[0])
-        self.set_angle(1, angles[1])
-        self.set_angle(2, angles[2])
+        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])
+            #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])
+        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])
 
     def reset(self, calibrated=False):
         if calibrated: