It's amazing to have made a robotic arm so easily, and with just a bit of code, we are now able to control it the way we want. However, there is one problem you might have noticed, that is, the robot is moving the way we want but not at the speed at which we want it to move. This is a very common problem while using the digital PWM-based servos.
These servos do not have a built-in speed control. Their control system is programmed to move the servo as fast as they can to reach the goal position. Hence, to control the speed, we will have to play with the program itself and give it a smooth linear progression.
The speed control can be done through a few different techniques. So, without much talking, let's go and see the code. Before you write it, read it and go through the code once and then see the following explanation to it. Thereafter, you will have a better idea of what we are doing. This will make writing the code faster and easier. So, let's take a look at it:
import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BCM)
GPIO.setup(14,GPIO.OUT)
GPIO.setup(16,GPIO.OUT)
GPIO.setup(18,GPIO.OUT)
GPIO.setup(20,GPIO.OUT)
GPIO.setup(21,GPIO.OUT)
GPIO.setup(22,GPIO.OUT)
GPIO.setwarnings(False)
pwm1 = GPIO.PWM(14, 50)
pwm2 = GPIO.PWM(16, 50)
pwm3 = GPIO.PWM(18, 50)
pwm4 = GPIO.PWM(20, 50)
pwm5 = GPIO.PWM(21, 50)
pwm6 = GPIO.PWM(22, 50)
pwm1.start(0)
pwm2.start(0)
pwm3.start(0)
pwm4.start(0)
pwm5.start(0)
pwm6.start(0)
def cvt_angle(angle):
dc = float(angle/90) + 0.5
return dc
prev0 = 90
prev1 = 90
prev2 = 90
prev3 = 90
prev4 = 90
prev5 = 90
pwm1.ChangeDutyCycle(cvt_angle(prev0))
pwm2.ChangeDutyCycle(cvt_angle(prev1))
pwm3.ChangeDutyCycle(cvt_angle(prev2))
pwm4.ChangeDutyCycle(cvt_angle(prev3))
pwm5.ChangeDutyCycle(cvt_angle(prev4))
pwm6.ChangeDutyCycle(cvt_angle(prev5))
while True:
a = raw_input("enter a list of 6 values for motor 1")
b = raw_input("enter a list of 6 values for motor 2")
c = raw_input("enter a list of 6 values for motor 3")
d = raw_input("enter a list of 6 values for motor 4")
e = raw_input("enter a list of 6 values for motor 5")
f = raw_input("enter a list of 6 values for motor 6")
speed = raw_input("enter one of the following speed 0.1, 0.2, 0.5, 1")
for i in range(6):
while prev0 =! a[i] and prev1 =! b[i] and prev2 =! c[i] and prev3 =! d[i] and prev4 =! e[i] and prev 5 =! f[i]
if a[i] > 10 and a[i]< 180 :
if prev0 > a[i]
prev0 = prev0 - speed
if prev0 < a[i]
prev0 = prev0 + speed
if prev0 = a[i]
prev0 = prev0
pwm1.ChangeDutyCycle(cvt_angle(prev0))
if b[i] > 10 and b[i] < 180:
if prev2 > b[i]
prev2 = prev2 - speed
if prev2 < b[i]
prev2 = prev2 + speed
if prev2 = b[i]
prev2 = prev2
pwm2.ChangeDutyCycle(cvt_angle(b[i]))
if c[i] > 10 and c[i] < 180:
if prev3 > c[i]
prev3 = prev3 - speed
if prev3 < c[i]
prev3 = prev3 + speed
if prev3 = c[i]
prev3 = prev3
pwm3.ChangeDutyCycle(cvt_angle(c[i]))
if d[i] > 10 and d[i] < 180:
if prev4 > d[i]
prev4 = prev4 - speed
if prev4 < d[i]
prev4 = prev4 + speed
if prev4 = d[i]
prev4 = prev4
pwm4.ChangeDutyCycle(cvt_angle(d[i]))
if e[i] > 10 and e[i] < 180:
if prev5 > e[i]
prev5 = prev5 - speed
if prev0 < e[i]
prev5 = prev5 + speed
if prev5 = e[i]
prev5 = prev5
pwm5.ChangeDutyCycle(cvt_angle(e[i]))
if f[i] > 10 and f[i] < 180:
if prev6 > f[i]
prev6 = prev6 - speed
if prev6 < f[i]
prev6 = prev6 + speed
if prev6 = f[i]
prev6 = prev6
pwm6.ChangeDutyCycle(cvt_angle(f[i]))
flag = 0
In this program, there are quite a few things. We should go through them one by one to have an understanding of it. So, let's see what we are doing:
prev0 = 90
prev1 = 90
prev2 = 90
prev3 = 90
prev4 = 90
prev5 = 90
Here, we have defined six new variables with the name prev0 to prev5 and all of them have been allowed a value of 90. The term prev here stands for previous, so this will be there to indicate the previous value.
while prev0 =! a[i] and prev1 =! b[i] and prev2 =! c[i] and prev3 =! d[i] and prev4 =! e[i] and prev 5 =! f[i]
After the code line for i in range 6, we have the preceding line of code, which is basically checking the value of a[i] with prev0. Similarly, it is checking the values of b[i] with prev1 and so on. Until the time all of these are not true the while loop will be true and will loop the program inside it until the condition is not false. That is, all the prev values are exactly equal to the values of the corresponding values of the list.
Again, this may seem a little odd to you, but, trust me, it will be quite useful, which we will see in a while:
if a[i] > 10 and a[i]< 180 :
if prev0 > a[i]
prev0 = prev0 - speed
if prev0 < a[i]
prev0 = prev0 + speed
if prev0 = a[i]
prev0 = prev0
pwm1.ChangeDutyCycle(cvt_angle(prev0))
Now, here comes the real deal. This is the main program that will control the speed of the servo. In this, the first line is simple; it will check whether the value given to it is valid, that is, between the safe limits. Once that is done, it will then check whether the value of a[Ii] is less than or greater than the previous value. If it is greater than a[i], then it will take in the previous value and decrement it with the speed specified by the user. If it is less than the value of a[i], then it will increment the previous value with the speed specified.
So, if you look at it, the code is simply incrementing or decrementing the previous value every time the while loop is running. Now, the while loop will run until the value of prev is equal to the corresponding list value. That is, the loop will keep incrementing the value until it reaches the specified position.
Hence, lower the value of the speed, lower will be the increments every single time, thereby slowing down the speed all together.
This is the same process which will happen for all other servos as well. It may sound very complicated, but it is not! Programming is easy and will continue to remain easy each time you break it down into small pieces and understand them one by one!