Merge pull request #10 from SentryCoderDev/feature/Jetson-Integration

First stage of Jetson integration

Author: SentryCoderDev

config/jetservo.yml

@@ -0,0 +1,24 @@
+jetservo:
+  conf:
+    servo1: { id: 1, pin: 17, range: [0, 180], start: 0 }
+    servo2: { id: 2, pin: 18, range: [0, 180], start: 0 }
+  docs:
+    description: "The jetservo module is used to control servos on the Jetson Nano."
+    events:
+      sub:
+        - jetservo:move:
+            description: "Move both servos to the same absolute position."
+            args:
+              - angle: integer
+        - jetservo:inversermove:
+            description: "Move servos in inverse directions."
+            args:
+              - angle: integer
+        - jetservo:left_move:
+            description: "Move the left servo to an absolute position."
+            args:
+              - angle: integer
+        - jetservo:right_move:
+            description: "Move the right servo to an absolute position."
+            args:
+              - angle: integer

config/piservo.yml

@@ -19,12 +19,12 @@
 # Import modules
 from modules.config import Config
 from modules.actuators.servo import Servo
-from modules.actuators.piservo import PiServo
+from modules.actuators.jetservo import JetsonServo
 from modules.animate import Animate
 # from modules.power import Power
 # from modules.keyboard import Keyboard
 # from modules.gamepad import Gamepad
-from modules.sensor import Sensor
++from modules.jetsensor import JetsonMotionSensor
 # try:
 #     from modules.hotword import HotWord
 # except ModuleNotFoundError as e:
@@ -40,7 +40,7 @@
 # from modules.battery import Battery
 from modules.braillespeak import Braillespeak
 from modules.buzzer import Buzzer
-from modules.pitemperature import PiTemperature
++from modules.jettemperature import JetsonTemperature
 from modules.osc_module import StartOSCServer
 
 from modules.translator import Translator
@@ -88,11 +88,11 @@ def main():
         s = servo_conf[key]
         servos[key] = Servo(s['pin'], key, s['range'], s['id'], start_pos=s['start'])
 
-    piservos = dict()
-    piservo_conf = Config.get('piservo','conf')
-    for key in piservo_conf:
-        s = piservo_conf[key]
-        piservos[key] = PiServo(s['pin'], s['range'], start_pos=s['start'])
+    jetservos = dict()
+    jetservo_conf = Config.get('jetservo','conf')
+    for key in jetservo_conf:
+        s = jetservo_conf[key]
+        jetservos[key] = JetsonServo(s['pin'], s['range'], start_pos=s['start'])
 
     # pub.sendMessage('log', msg="[Main] Starting pan test")
     # pub.sendMessage('servo:pan:mvabs', percentage=0)
@@ -118,7 +118,7 @@ def main():
     # tts = TTS(translator=translator)
 
     if Config.get('motion','pin') != '':
-        motion = Sensor(Config.get('motion','pin'))
+        motion = JetsonMotionSensor(Config.get('motion','pin'))
 
     pub.sendMessage('tts', msg='I am awake.')
     pub.sendMessage('speak', msg='hi')
@@ -163,7 +163,7 @@ def main():
         # keyboard = Keyboard()
 
     # gamepad = Gamepad()
-    temp = PiTemperature()
+    temp = JetsonTemperature()
 
     # Voice
     # if Config.get('hotword', 'model') != '':

main.py

@@ -0,0 +1,61 @@
+import Jetson.GPIO as GPIO
+from time import sleep
+from pubsub import pub
+
+class JetsonServo:
+    def __init__(self, left_pin, right_pin, **kwargs):
+        self.left_pin = left_pin
+        self.right_pin = right_pin
+        self.start = kwargs.get('start_pos', 0)
+        self.setup_gpio()
+        self.left_servo = GPIO.PWM(left_pin, 50)  # Left servo pin, 50Hz
+        self.right_servo = GPIO.PWM(right_pin, 50)  # Right servo pin, 50Hz
+        self.left_servo.start(self.start)
+        self.right_servo.start(self.start)
+        
+        pub.subscribe(self.move, 'jetservo:move') # All jetservo movement
+        pub.subscribe(self.invert_move, 'jetservo:inversermove')  # Inverse jetservo movement
+        pub.subscribe(self.move_left, 'jetservo:left_move')  # Left jetservo
+        pub.subscribe(self.move_right, 'jetservo:right_move')  # Right jetservo
+
+    def setup_gpio(self):
+        GPIO.setmode(GPIO.BCM)  # Set GPIO numbering mode to BCM
+        GPIO.setup(self.left_pin, GPIO.OUT)  # Set left pin as output
+        GPIO.setup(self.right_pin, GPIO.OUT)  # Set right pin as output
+
+    def move(self, angle):
+        left_duty = self.angle_to_duty(angle)
+        right_duty = self.angle_to_duty(angle)
+        self.left_servo.ChangeDutyCycle(left_duty)
+        self.right_servo.ChangeDutyCycle(right_duty)
+        sleep(1)  # @TODO: Remove this sleep
+
+    def invert_move(self, angle):
+        left_duty = self.angle_to_duty(angle)
+        right_duty = self.angle_to_duty(180 - angle)  # Inverse movement for right servo
+        self.left_servo.ChangeDutyCycle(left_duty)
+        self.right_servo.ChangeDutyCycle(right_duty)
+        sleep(1)  # @TODO: Remove this sleep
+
+    def move_left(self, angle):
+        duty = self.angle_to_duty(angle)
+        GPIO.output(self.left_pin, GPIO.HIGH)  # Set left servo pin to HIGH
+        self.left_servo.ChangeDutyCycle(duty)
+        sleep(1)  # Wait for the servo to complete movement
+        GPIO.output(self.left_pin, GPIO.LOW)  # Set left servo pin to LOW
+
+    def move_right(self, angle):
+        duty = self.angle_to_duty(angle)
+        GPIO.output(self.right_pin, GPIO.HIGH)  # Set right servo pin to HIGH
+        self.right_servo.ChangeDutyCycle(duty)
+        sleep(1)  # Wait for the servo to complete movement
+        GPIO.output(self.right_pin, GPIO.LOW)  # Set right servo pin to LOW
+
+    def angle_to_duty(self, angle):
+        duty = angle / 18 + 2  # Map angle (0 to 180) to duty cycle (2 to 12)
+        return duty
+
+    def cleanup(self):
+        self.left_servo.stop()
+        self.right_servo.stop()
+        GPIO.cleanup()  # Clean up GPIO pins

modules/actuators/jetservo.py

@@ -0,0 +1,19 @@
+import Jetson.GPIO as GPIO
+from time import sleep
+from pubsub import pub
+
+class JetsonMotionSensor:
+  def __init__(self, pin, **kwargs):
+    self.pin = pin
+    self.value = None
+    GPIO.setup(self.pin, GPIO.IN)  # Set pin as input
+
+  def loop(self):
+    if self.read():
+      pub.sendMessage('motion')
+    sleep(0.1)  # Adjust delay as needed
+
+  def read(self):
+    self.value = GPIO.input(self.pin)
+    return self.value
+

modules/actuators/piservo.py

@@ -0,0 +1,39 @@
+import os
+from pubsub import pub
+
+class JetsonTemperature:
+    WARNING_TEMP = 80
+    THROTTLED_TEMP = 85
+    AVG_TEMP = 70
+
+    def __init__(self, sensor_file='/sys/devices/virtual/thermal/thermal_zone0/temp'):
+        self.sensor_file = sensor_file
+        pub.subscribe(self.monitor, 'loop:10')
+
+    def read(self):
+        with open(self.sensor_file, 'r') as f:
+            temp_str = f.readline().strip()
+            return float(temp_str) / 1000  # Convert millidegrees Celsius to degrees Celsius
+
+    def monitor(self):
+        val = self.read()
+        pub.sendMessage('log', msg='[TEMP] ' + str(val))
+        if val >= JetsonTemperature.THROTTLED_TEMP:
+            pub.sendMessage('led:full', color='red') # WARNING
+        # else:
+            # pub.sendMessage('led', identifiers='top5', color=self.map_range(round(val)))  # right
+
+
+    def map_range(self, value):
+        # Cap range for LED
+        if value > JetsonTemperature.WARNING_TEMP:
+            value = JetsonTemperature.WARNING_TEMP
+        if value < JetsonTemperature.AVG_TEMP:
+            value = JetsonTemperature.AVG_TEMP
+
+        # translate range (STARTUP_TEMP to WARNING_TEMP) to (100 to 0) (green is cool, red is hot)
+        OldRange = (JetsonTemperature.AVG_TEMP - JetsonTemperature.WARNING_TEMP)
+        NewRange = (100 - 0)
+        val = (((value - JetsonTemperature.WARNING_TEMP) * NewRange) / OldRange) + 0
+        return val
+

modules/jetsensor.py

@@ -28,18 +28,19 @@
 from pubsub import pub
 pub.sendMessage('behaviour', type=Personality.INPUT_TYPE_FUN)
 """
+from jetsensor import JetsonMotionSensor  
 
 class Personality:
 
-
     def __init__(self, **kwargs):
         self.mode = kwargs.get('mode', Config.MODE_LIVE)
         self.state = Config.STATE_SLEEPING
         self.eye = 'blue'
+        self.boredom_level = 0  
 
         pub.subscribe(self.loop, 'loop:1')
         pub.subscribe(self.process_sentiment, 'sentiment')
-        
+        pub.subscribe(self.on_motion_detected, 'motion')  
         behaviours = { 'boredom': Boredom(self),
                        'dream': Dream(self),
                        'faces': Faces(self),
@@ -51,24 +52,53 @@ def __init__(self, **kwargs):
                        'sentiment': Sentiment(self)}
 
         self.behaviours = SimpleNamespace(**behaviours)
+        self.motion_sensor = JetsonMotionSensor(pin=17)
 
     def loop(self):
         # pub.sendMessage('speech', msg="Hello, I am happy") # for testing sentiment responses
         if not self.is_asleep() and not self.behaviours.faces.face_detected and not self.behaviours.motion.is_motion() and not self.behaviours.objects.is_detected:
+            self.boredom_level += 1
             self.set_eye('red')
+        else:
+            self.boredom_level = 0
+
+        
+        if self.boredom_level > 10:
+            self.random_servo_movement()
+        
+     
+        self.motion_sensor.loop()
 
+       
         if self.state == Config.STATE_ALERT and self.lt(self.behaviours.faces.last_face, self.past(2*60)) and self.lt(self.behaviours.objects.last_detection, self.past(2*60)):
             # reset to idle position after 2 minutes inactivity
             pub.sendMessage('animate', action="wake")
             self.set_state(Config.STATE_IDLE)
 
     def process_sentiment(self, score):
-        pub.sendMessage('log', msg="[Personality] Sentiment: " + str(score))
-        if score > 0:
-            pub.sendMessage('piservo:move', angle=0)
-        else:
-            pub.sendMessage('piservo:move', angle=40)
-        
+       
+        if self.boredom_level <= 10:
+            pub.sendMessage('log', msg="[Personality] Sentiment: " + str(score))
+            if score > 0:
+                pub.sendMessage('jetservo:left_move', angle=0)
+                pub.sendMessage('jetservo:right_move', angle=0)
+            else:
+                pub.sendMessage('jetservo:left_move', angle=40)
+                pub.sendMessage('jetservo:right_move', angle=40)
+
+    def random_servo_movement(self):        
+        pub.sendMessage('log', msg="[Personality] Random Servo Movement due to boredom")
+        left_angle = randint(-30, 30)
+        right_angle = randint(-30, 30)
+        pub.sendMessage('jetservo:left_move', angle=left_angle)
+        pub.sendMessage('jetservo:right_move', angle=right_angle)
+
+    def on_motion_detected(self):
+       
+        pub.sendMessage('log', msg="[Personality] Motion detected, servos moving up like ears")
+        pub.sendMessage('jetservo:left_move', angle=-90)
+        pub.sendMessage('jetservo:right_move', angle=-90)
+
     def set_eye(self, color):
         if self.eye == color:
             return
@@ -79,36 +109,35 @@ def set_eye(self, color):
     def set_state(self, state):
         if self.state == state:
             return
-
         pub.sendMessage('log', msg="[Personality] State: " + str(state))
         if state == Config.STATE_SLEEPING:
             pub.sendMessage("sleep")
             pub.sendMessage("animate", action="sleep")
             pub.sendMessage("animate", action="sit")
             pub.sendMessage("led:off")
             pub.sendMessage("led", identifiers=['status1'], color="off")
-            pub.sendMessage('piservo:move', angle=0)
+            pub.sendMessage('jetservo:move', angle=0)
         elif state == Config.STATE_RESTING:
             pub.sendMessage('rest')
             pub.sendMessage("animate", action="sit")
             pub.sendMessage("animate", action="sleep")
             self.set_eye('blue')
             pub.sendMessage("led", identifiers=['status1'], color="red")
-            pub.sendMessage('piservo:move', angle=-40)
+            pub.sendMessage('jetservo:move', angle=-40)
         elif state == Config.STATE_IDLE:
             if self.state == Config.STATE_RESTING or self.state == Config.STATE_SLEEPING:
                 pub.sendMessage('wake')
                 pub.sendMessage('animate', action="wake")
             pub.sendMessage('animate', action="sit")
             pub.sendMessage("led", identifiers=['status1'], color="green")
-            pub.sendMessage('piservo:move', angle=-20)
+            pub.sendMessage('jetservo:move', angle=-20)
             self.set_eye('blue')
         elif state == Config.STATE_ALERT:
             if self.state == Config.STATE_RESTING or self.state == Config.STATE_SLEEPING:
                 pub.sendMessage('wake')
                 pub.sendMessage('animate', action="wake")
             # pub.sendMessage('animate', action="stand")
-            pub.sendMessage('piservo:move', angle=0)
+            pub.sendMessage('jetservo:move', angle=0)
             pub.sendMessage("led", identifiers=['status1'], color="blue")
         self.state = state
 
@@ -122,4 +151,4 @@ def lt(self, date, compare):
         return date is None or date < compare
 
     def past(self, seconds):
-        return datetime.now() - timedelta(seconds=seconds)
+        return datetime.now() - timedelta(seconds=seconds)