CODEWITHSUNDEEP
pythontrigonometry
Kingdavidek
Kingdavidek

Reputation: 29

Make servos move in sinusoids (Python)

I am trying to find a way to make my servos move like a sin wave. I have managed to make them jump from one position to another by I don't know how to make them move smoothly in sinusoids at a defined frequency and amplitude :/

The code for sending commands to servos is below, I use the Adafruit servo library to send commands to my servos. 

#!/usr/bin/python

from Adafruit_PWM_Servo_Driver import PWM
import time

# ===========================================================================
# Example Code
# ===========================================================================

# Initialise the PWM device using the default address
pwm = PWM(0x40)
# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)

servoMin = 150  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096

def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(60)                        # Set frequency to 60 Hz
while (True):
  # Change speed of continuous servo on channel O
  pwm.setPWM(0, 0, servoMin)
  time.sleep(1)
  pwm.setPWM(0, 0, servoMax)
  time.sleep(1)

Would really appreciate the help!

Cheers,

David

Upvotes: 0

Views: 618

Answers (1)

Zaur Nasibov
Zaur Nasibov

Reputation: 22679

If I understood correctly, all you need is to produce values between servoMin and servoMax in a sin-wave manner.

Something like (Updated):

from math import pi, sin

MS_IN_SECOND = 1000

# lower and upper amplitude value boundaries
lb = servoMin
ub = servoMax
zero = (lb + ub) / 2

max_amplitude = ub - zero

# set the amplitude, comes with a safeguard
amplitude = min(max_amplitude, 100) 

# set the frequency, i.e. how many times per second (actual, or scaled)
# the wave crosses 'zero'
freq = 1

# sleep duration (in milliseconds!) is a resolution for
# the values produced in 1 second period. If you need more 
# discrete points - decrease the duration, and vise-verse.
sleep_duration = 50

# automatically calculated step - the difference between
# two discrete points of the wave.
step = 2 * pi * freq / (MS_IN_SECOND / sleep_duration)

i = 0
while(True):
    val = zero + amplitude * sin(i)
    pwm.setPWM(0, 0, int(val))
    i += step
    sleep(sleep_duration)

Upvotes: 1

Related Questions