Raspbian For Robots: Custom initialization scripts/code?

Given:

  1. A 'bot running a recent version of Raspbian for Robots.
  2. The existence of various sensors/actuators.
  3. The desire to have these sensors/actuators initialized to a known state/position at system startup.

Bonus Question:

  1. Add the ability to reset lights, sensors, actuators to a known state at the conclusion of a particular software routine, WITHOUT having to insert a specialized shutdown script at the end of every program.

Example:

At system startup, I would like for Charlie’s head servo to center - and both the distance sensor and IMU to initialize and come on-line.

I would also like to have “Dex’s” lights flash in a particular way prior to the Wi-Fi coming on to let me know that this init routine is complete.

Is there a way to do this? A script or routine in init.d or rc.d somewhere?

What would be the best way to do this?
A shell script or a monolithic compiled Python routine that has all needed libraries statically linked?

What say ye?

Jim “JR”

One shot Initializing stuff at startup is nicely handled by scheduling a startup.py script using cron. I edit the root crontab:

sudo crontab -e 

to include the scripts I want to run when the system boots or reboots:

# Edit this file to introduce tasks to be run by cron.
#
# Each task to run has to be defined through a single line
# indicating with different fields when the task will be run
# and what command to run for the task
#
# To define the time you can provide concrete values for
# minute (m), hour (h), day of month (dom), month (mon),
# and day of week (dow) or use '*' in these fields (for 'any').#
# Notice that tasks will be started based on the cron's system
# daemon's notion of time and timezones.
#
# Output of the crontab jobs (including errors) is sent through
# email to the user the crontab file belongs to (unless redirected).
#
# For example, you can run a backup of all your user accounts
# at 5 a.m every week with:
# 0 5 * * 1 tar -zcf /var/backups/home.tgz /home/
#
# For more information see the manual pages of crontab(5) and cron(8)
#
# m h  dom mon dow   command
@reboot /home/pi/Carl/plib/loglife.py&
@reboot sleep 60 && /home/pi/Carl/nohup_juicer.sh
@reboot sleep 60 && /home/pi/Carl/nohup_wheellog.sh

Notice the use of full paths, and notice the first command has the ampersand at the end to put it into the background to continue running. The last two commands are examples of once and done scripts that are run 60 seconds after a boot (to allow the Internet time query to occur before starting stuff that needs an accurate time).

Sensor code packaging can get tricky. I put the tilt-pan and distance sensor in their own modules with the expectation that the “instance” of each will be in the using module.

For example, I have a convenience script called center.py which centers the tilt-pan:

#!/usr/bin/python
#
# center.py    Center Tilt and Pan Servos
#

#
from __future__ import print_function
from __future__ import division

# import the modules

import sys
sys.path
sys.path.append('/home/pi/Carl/plib')
import tiltpan
from time import sleep
import easygopigo3
import myconfig

# ##### MAIN ######

def main():
  egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
  myconfig.setParameters(egpg)
  tp = tiltpan.TiltPan(egpg)

  tp.tiltpan_center()
  print("tiltpan centered")
  sleep(0.2)
  tp.off()
  print("tiltpan off")

if __name__ == "__main__":
    main()

My tilt and pan servos are combined as a TiltPan() class, which maintains the individual tilt servo instance and pan servo instance as class variables.

The distance sensor module only has helper functions, no class is defined.

So a “main” robot script (center.py in this example) will always instantiate an EasyGoPiGo3(use_mutex=True) class, and then pass that instance to the tilt-pan module to create and return a tilt-pan class instance (tp).

The tiltpan.py module also has some tilt-pan behaviors that move the assembly in a “yes” (up and down), and a “no” (left and right), and an “I don’t know” (roll your eyes in a circle) in addition to positioning and returning the current position of each (if have not been turned off…)

#!/usr/bin/python
#
# tiltpan.py    Tilt and Pan Servo Management Class
#
# Pan Servo:  PAN_LEFT_LIMIT  = 0, PAN_CENTER  = 90, PAN_RIGHT_LIMIT = 180   (for GoPiGo3 servo compatibility)
# Tilt Servo: TILT_DOWN_LIMIT = -90, TILT_CENTER = 0, TILT_UP_LIMIT   = 90
#
# Methods:
#  TiltPan(egpg)          # creates class object
#  tilt(pos = tilt_pos)
#  pan(pos = pan_pos)
#  tiltpan_center()       # convenience tilt(TILT_CENTER), pan(PAN_CENTER)
#  center()               # same as tiltpan_center()
#  off()                  # turn both servos off / non-holding position (sets pos to UNKNOWN)
#  nod_yes(spd=0.03)
#  nod_no(spd=0.02)
#  nod_IDK(spd=0.02)
#  get_tilt_pos()
#  get_pan_pos()
#
#
"""

Usage:

import tiltpan
import easygopigo3
import myconfig
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
myconfig.setParameters(egpg)
tp = tiltpan.TiltPan(egpg)
tp.tiltpan_center()
tp.tilt(tiltpan.TILT_CENTER) # -90 to +90
tp…pan(tiltpan.PAN_CENTER)
print tp.get_pan_pos(), tp.get_tilt_pos()
tp.tiltpan.off()

"""

#
from __future__ import print_function
from __future__ import division

# import the modules

import sys
sys.path
sys.path.append('/home/pi/Carl/plib')

from time import sleep
import signal
import os
import myPyLib
import speak
from datetime import datetime
from math import sin,cos,radians
import myconfig
import easygopigo3

TILT_PORT = "SERVO2"
PAN_PORT  = "SERVO1"

PAN_LEFT_LIMIT = 0
PAN_RIGHT_LIMIT = 180
PAN_CENTER = 90

TILT_DOWN_LIMIT = -65
TILT_CENTER = 0
TILT_UP_LIMIT = 65

UNKNOWN = 999

class TiltPan():
    tilt_position = TILT_CENTER
    pan_position  = PAN_CENTER
    ts = None
    ps = None
    def __init__(self, egpg):
        TiltPan.ts = egpg.init_servo(TILT_PORT)
        TiltPan.ps = egpg.init_servo(PAN_PORT)

    def get_tilt_pos(self):
        return TiltPan.tilt_position

    def get_pan_pos(self):
       return TiltPan.pan_position

    def tiltpan_center(self):
      TiltPan.tilt_position = TILT_CENTER
      TiltPan.pan_position = PAN_CENTER
      self.tilt()
      sleep(0.25)  # delay to limit current draw, SG90 spec: 0.12 sec/60deg
      self.pan()
      sleep(0.25)  # delay to ensure action incase next method is off()

    def center(self):
      self.tiltpan_center()

    def off(self):          # turn both servo PWM freq to 0 to stop holding position
      TiltPan.ps.gpg.set_servo(TiltPan.ps.portID, 0)
      TiltPan.ts.gpg.set_servo(TiltPan.ts.portID, 0)
      TiltPan.tilt_position = UNKNOWN
      TiltPan.pan_position = UNKNOWN

    def tilt(self,tgt=None):
      if tgt != None: TiltPan.tilt_position = tgt
      if (TiltPan.tilt_position > TILT_UP_LIMIT):
          TiltPan.tilt_position = TILT_UP_LIMIT
          print("tilt(tgt) > TILT_UP_LIMIT")
      if (TiltPan.tilt_position < TILT_DOWN_LIMIT):
          TiltPan.tilt_position = TILT_DOWN_LIMIT
          print("tilt(tgt) < TILT_DOWN_LIMIT")
      TiltPan.ts.rotate_servo(-TiltPan.tilt_position+90)


    def pan(self,tgt=None):
      if tgt != None: TiltPan.pan_position = tgt
      if (TiltPan.pan_position > PAN_RIGHT_LIMIT):
         TiltPan.pan_position = PAN_RIGHT_LIMIT
         print("pan(tgt) > PAN_RIGHT_LIMIT")
      if (TiltPan.pan_position < PAN_LEFT_LIMIT):
         TiltPan.pan_position = PAN_LEFT_LIMIT
         print("pan(tgt) < PAN_LEFT_LIMIT")
      TiltPan.ps.rotate_servo(180-TiltPan.pan_position)

    def nod_yes(self,spd=0.03):
        NOD_UP_LIMIT = 20
        NOD_DN_LIMIT = -20
        TIMES_TO_NOD = 2
        for i in range(0,TIMES_TO_NOD):
            for angle in range(TILT_CENTER, NOD_UP_LIMIT+1, 5):
                    self.tilt(angle)
                    sleep(spd)
            for angle in range(NOD_UP_LIMIT, NOD_DN_LIMIT-1, -5):
                    self.tilt(angle)
                    sleep(spd)
            for angle in range(NOD_DN_LIMIT, TILT_CENTER+1, 5):
                    self.tilt(angle)
                    sleep(spd)
        self.tiltpan_center()

    def nod_no(self,spd=0.02):
        NOD_LEFT_LIMIT = 70
        NOD_RIGHT_LIMIT = 110
        TIMES_TO_NOD = 2
        self.tiltpan_center()
        for i in range(0,TIMES_TO_NOD):
            for angle in range(PAN_CENTER, NOD_LEFT_LIMIT-1, -5):
                    self.pan(angle)
                    sleep(spd)
            for angle in range(NOD_LEFT_LIMIT, NOD_RIGHT_LIMIT+1, 5):
                    self.pan(angle)
                    sleep(spd)
            for angle in range(NOD_RIGHT_LIMIT, PAN_CENTER-1, -5):
                    self.pan(angle)
                    sleep(spd)
        self.tiltpan_center()

    def nod_IDK(self,spd=0.02):
        for radius in range(30,60+1,30):
            for angle in range(0,360+1,2):
                tiltp =  int( radius * sin( radians(angle) ))
                panp  =  int( radius * cos( radians(angle) )) + 90
                self.tilt(tiltp)
                self.pan(panp)
                sleep(spd)
        self.tiltpan_center()



# ##### MAIN ######

def handle_ctlc():
  global tp
  tp.tiltpan_center()
  print("tiltpan.py: handle_ctlc() executed")

def main():
  global tp
  try:
    egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
    tp = TiltPan(egpg)
    # #### SET CNTL-C HANDLER
    myPyLib.set_cntl_c_handler(handle_ctlc)

    while True:
        print("tiltpan centered")
        tp.tiltpan_center()
        sleep(5)
        print("pan(45)")
        tp.pan(45)
        sleep(5)
        print("pan(135)")
        tp.pan(135)
        sleep(5)
        print("pan(PAN_CENTER)")
        tp.pan(PAN_CENTER)
        print("tilt(45)")
        tp.tilt(45)
        sleep(5)
        print("tilt(-45)")
        tp.tilt(-45)
        sleep(5)
        print("tilt(TILT_CENTER)")
        tp.tilt(TILT_CENTER)

        tp.tiltpan_center()
        print("pan_position:",tp.get_pan_pos() )
        print('UP too far')
        tp.tilt(90)
        print("tilt_position:",tp.get_tilt_pos() )
        sleep(5)
        print('DOWN too far')
        tp.tilt(-90)
        print("tilt_position:",tp.get_tilt_pos() )
        sleep(5)
        print("YES")
        tp.nod_yes()
        sleep(2)
        print("NO")
        tp.nod_no()
        sleep(2)
        print("IDK")
        tp.nod_IDK()
        sleep(2)
        print("Turning tiltpan servos off")
        tp.off()
        sleep(5)
        print("tilt_position(999=UNKNOWN):",tp.get_tilt_pos() )
        print("pan_position (999=UNKNOWN):",tp.get_pan_pos() )
    #end while
  except SystemExit:
    print("tiltpan.py: exiting")

if __name__ == "__main__":
    main()