For the past few weeks, we have been trying to use an automatic control technique called PD control (proportional–derivative) to control the position of the EC motor. In PD control, we modify the proportional gain and derivative gain coefficients of the function to maximize the smoothness of the motor movements
oo=-0.4*(ang-Dang)-310*Vel
This is the key code that controls the distance and time (therefore velocity) of the motor. The variable "ang" is the read of the motor's position and the "Dang" is the desired angle that the motor tries to achieve. The first coefficient (0.4) controls the slope of the function. The bigger the number, the more smooth the position-versus-time graph will get. The smoothness, however, will eventually flatten out the curve and make the motor's movement unnoticeable once the number gets too big. Therefore, we needed to tune this value and find the optimal number that allows the motor to move to the desired angle.
However, even though we tried many different values, the movement of the motor still was not very continuous. Below is the position-versus-time graph of one of our better trails. 
After discussing with our supervisor, we decided to employ another technique called Torque Control. By directly manipulating the amount of current that goes into the motor, we can more effectively control the movement of the motor, rather than position. To do this, we first retuned the motors and configured it for the torque control. We then went tested and found out the optimal torque value for each of the three motors. Because each finger contains a slightly different amount of mass, they all differ in the amount of static torque. The wrist motor, however, contains a higher nominal torque than the other two motors. Therefore, it only requires 2% of torque to overcome the static torque. Overall, torque control allowed the motors to move drastically smoother compared to the previous PD control. Note: conventionally, PD control may be a better choice for this application but we would need to be using a faster controller or using a faster-compiled language like C or C++.
In order to improve the efficiency to control the robotic arm, we decided to tidy our code into functions. Below is a snippet of our code:
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
from __future__ import unicode_literals
import nidaqmx
import numpy as np
import matplotlib.pyplot as plt
import sys
import time
import signal
from nidaqmx.constants import AcquisitionType, TaskMode, CountDirection, Edge
from nidaqmx._task_modules.channels.ci_channel import CIChannel
from nidaqmx._task_modules.channel_collection import ChannelCollection
from nidaqmx.constants import AngleUnits
from nidaqmx.constants import AngularVelocityUnits
from nidaqmx.constants import EncoderType
import ctypes
import six
import warnings
from nidaqmx._lib import lib_importer, ctypes_byte_str, c_bool32
from nidaqmx._task_modules.channels.channel import Channel
from nidaqmx._task_modules.export_signals import ExportSignals
from nidaqmx._task_modules.in_stream import InStream
from nidaqmx._task_modules.read_functions import (
_read_analog_f_64, _read_digital_lines, _read_digital_u_32, _read_ctr_freq,
_read_ctr_time, _read_ctr_ticks, _read_counter_u_32_ex,
_read_counter_f_64_ex)
from nidaqmx._task_modules.timing import Timing
from nidaqmx._task_modules.triggers import Triggers
from nidaqmx._task_modules.out_stream import OutStream
from nidaqmx._task_modules.ai_channel_collection import (
AIChannelCollection)
from nidaqmx._task_modules.ao_channel_collection import (
AOChannelCollection)
from nidaqmx._task_modules.ci_channel_collection import (
CIChannelCollection)
from nidaqmx._task_modules.co_channel_collection import (
COChannelCollection)
from nidaqmx._task_modules.di_channel_collection import (
DIChannelCollection)
from nidaqmx._task_modules.do_channel_collection import (
DOChannelCollection)
from nidaqmx._task_modules.write_functions import (
_write_analog_f_64, _write_digital_lines, _write_digital_u_32,
_write_ctr_freq, _write_ctr_time, _write_ctr_ticks)
from nidaqmx.constants import (
AcquisitionType, ChannelType, UsageTypeCI, EveryNSamplesEventType,
READ_ALL_AVAILABLE, UsageTypeCO, _Save)
from nidaqmx.error_codes import DAQmxErrors
from nidaqmx.errors import (
check_for_error, is_string_buffer_too_small, DaqError, DaqResourceWarning)
from nidaqmx.system.device import Device
from nidaqmx.types import CtrFreq, CtrTick, CtrTime
from nidaqmx.utils import unflatten_channel_string, flatten_channel_string
def ReadAnalogInput(Dname,ch):
DD=Dname + '/ai' +str(ch)
with nidaqmx.Task() as task:
task.ai_channels.add_ai_voltage_chan(DD)
out=task.read()
return out
def WriteAnalogOutput(Dname,ch,value):
DD=Dname + '/ao' +str(ch)
with nidaqmx.Task() as task:
task.ao_channels.add_ao_voltage_chan(DD)
task.write(value)
def ReadDigitalInput(Dname,ch):
DD=Dname + '/port0/line' +str(ch)
with nidaqmx.Task() as task:
task.di_channels.add_di_chan(DD)
out=task.read()
return out
def WriteDigitalOutput(Dname,ch,value):
DD=Dname + '/port0/line' +str(ch)
with nidaqmx.Task() as task:
task.do_channels.add_do_chan(DD)
task.write(value)
def ReadAngle(Dname,ch):
DD=Dname + '/ctr' +str(ch)
with nidaqmx.Task() as task:
task.ci_channels.add_ci_ang_encoder_chan(counter = DD, decoding_type = EncoderType.X_4, units=AngleUnits.DEGREES, pulses_per_rev=20)
ang=task.read()
return ang
def VoltageOutput(Dname,Vel):
if Vel>0:
WriteAnalogOutput(Dname,0,Vel)
WriteDigitalOutput(Dname,1,True)
WriteDigitalOutput(Dname,0,False)
elif Vel<0:
WriteAnalogOutput(Dname,0,-Vel)
WriteDigitalOutput(Dname,0,True)
WriteDigitalOutput(Dname,1,False)
def MotorStop(Dname):
#WriteDigitalOutput(Dname,0,False)
#WriteDigitalOutput(Dname,1,False)
#WriteDigitalOutput(Dname,2,False)
#WriteDigitalOutput(Dname,3,False)
WriteDigitalOutput(Dname,4,False)
WriteDigitalOutput(Dname,5,False)
#WriteAnalogOutput(Dname,0,0)
#WriteAnalogOutput(Dname,1,0)
WriteAnalogOutput(Dname,2,0)
def allStop(Dname):
WriteDigitalOutput(Dname,0,False)
WriteDigitalOutput(Dname,1,False)
WriteDigitalOutput(Dname,2,False)
WriteDigitalOutput(Dname,3,False)
WriteDigitalOutput(Dname,4,False)
WriteDigitalOutput(Dname,5,False)
def handler(signal, frame):
MotorStop('Dev4')
sys.exit(0)
def main(LoopW=20, LoopT=20, LoopI=20, torqueW=0.2, torqueT=1.6, torqueI=1.1):
signal.signal(signal.SIGINT, handler)
Dname='Dev4'
maxLoop = max(LoopW, LoopT, LoopI)
print(maxLoop)
with nidaqmx.Task() as taskCNTIndex, nidaqmx.Task() as taskAOIndex, nidaqmx.Task() as taskDO0Index, nidaqmx.Task() as taskDO1Index,\
nidaqmx.Task() as taskCNTThumb, nidaqmx.Task() as taskAOThumb, nidaqmx.Task() as taskDO0Thumb, nidaqmx.Task() as taskDO1Thumb,\
nidaqmx.Task() as taskCNTWrist, nidaqmx.Task() as taskAOWrist, nidaqmx.Task() as taskDO0Wrist, nidaqmx.Task() as taskDO1Wrist:
#for Index
taskAOIndex.ao_channels.add_ao_voltage_chan('Dev4/ao1')
taskDO0Index.do_channels.add_do_chan('Dev4/port0/line2')
taskDO1Index.do_channels.add_do_chan('Dev4/port0/line3')
taskCNTIndex.ci_channels.add_ci_ang_encoder_chan(counter = 'Dev4/ctr1', decoding_type = EncoderType.X_4, units=AngleUnits.DEGREES, pulses_per_rev=512, initial_angle=0.0)
taskAOThumb.ao_channels.add_ao_voltage_chan('Dev4/ao0')
taskDO0Thumb.do_channels.add_do_chan('Dev4/port0/line0')
taskDO1Thumb.do_channels.add_do_chan('Dev4/port0/line1')
taskCNTThumb.ci_channels.add_ci_ang_encoder_chan(counter = 'Dev4/ctr0', decoding_type = EncoderType.X_4, units=AngleUnits.DEGREES, pulses_per_rev=512, initial_angle=0.0)
taskAOWrist.ao_channels.add_ao_voltage_chan('Dev4/ao2')
taskDO0Wrist.do_channels.add_do_chan('Dev4/port0/line4')
taskDO1Wrist.do_channels.add_do_chan('Dev4/port0/line5')
taskCNTWrist.ci_channels.add_ci_ang_encoder_chan(counter = 'Dev4/ctr2', decoding_type = EncoderType.X_4, units=AngleUnits.DEGREES, pulses_per_rev=512, initial_angle=0.0)
POSIndex=[0 for i in range(LoopI)]
VELIndex=[0 for i in range(LoopI)]
OOIndex=[0 for i in range(LoopI)]
taskAOIndex.start()
taskDO0Index.start()
taskDO1Index.start()
taskCNTIndex.start()
VelIndex=0
PangIndex=0
POSThumb=[0 for i in range(LoopT)]
VELThumb=[0 for i in range(LoopT)]
OOThumb=[0 for i in range(LoopT)]
taskAOThumb.start()
taskDO0Thumb.start()
taskDO1Thumb.start()
taskCNTThumb.start()
VelThumb=0
PangThumb=0
POSWrist=[0 for i in range(LoopW)]
VELWrist=[0 for i in range(LoopW)]
OOWrist=[0 for i in range(LoopW)]
taskAOWrist.start()
taskDO0Wrist.start()
taskDO1Wrist.start()
taskCNTWrist.start()
VelWrist=0
PangWrist=0
TIME=[0 for i in range(maxLoop)]
A=time.time()
for i in range(maxLoop):
# Index
if i < LoopI:
angIndex=taskCNTIndex.read()
VelIndex=0.95*VelIndex+0.05*(angIndex-PangIndex)/10
PangIndex=angIndex
ooIndex= torqueI
if ooIndex>0:
if ooIndex>10:
ooIndex=10
taskAOIndex.write(ooIndex)
taskDO0Index.write(True)
taskDO1Index.write(False)
else:
if ooIndex<-10:
ooIndex=-10
taskAOIndex.write(-ooIndex)
taskDO1Index.write(True)
taskDO0Index.write(False)
POSIndex[i]=angIndex
VELIndex[i]=VelIndex
# Thumb
if i < LoopT:
angThumb=taskCNTThumb.read()
VelThumb=0.95*VelThumb+0.05*(angThumb-PangThumb)/10
PangThumb=angThumb
ooThumb=torqueT
if ooThumb>0:
if ooThumb>10:
ooThumb=10
taskAOThumb.write(ooThumb)
taskDO0Thumb.write(True)
taskDO1Thumb.write(False)
else:
if ooThumb<-10:
ooThumb=-10
taskAOThumb.write(-ooThumb)
taskDO1Thumb.write(True)
taskDO0Thumb.write(False)
POSThumb[i]=angThumb
VELThumb[i]=VelThumb
if i < LoopW:
angWrist = taskCNTWrist.read()
VelWrist=0.95*VelWrist+0.05*(angWrist-PangWrist)/10
PangWrist=angWrist
ooWrist=torqueW #torque for wrist = 0.2/10
if ooWrist>0:
if ooWrist>10:
ooWrist=10
taskAOWrist.write(ooWrist)
taskDO0Wrist.write(True)
taskDO1Wrist.write(False)
else:
if ooWrist<-10:
ooWrist=-10
taskAOWrist.write(-ooWrist)
taskDO1Wrist.write(True)
taskDO0Wrist.write(False)
POSWrist[i]=angWrist
VELWrist[i]=VelWrist
TIME[i]=time.time()-A
"""
taskAOWrist.stop()
taskDO0Wrist.stop()
taskDO1Wrist.stop()
taskCNTWrist.stop()
"""
#MotorStop(Dname)
def wrist(torque, LoopN=300):
with nidaqmx.Task() as taskCNT, nidaqmx.Task() as taskAO, nidaqmx.Task() as taskDO0, nidaqmx.Task() as taskDO1, nidaqmx.Task():
taskAO.ao_channels.add_ao_voltage_chan('Dev4/ao2')
taskDO0.do_channels.add_do_chan('Dev4/port0/line4')
taskDO1.do_channels.add_do_chan('Dev4/port0/line5')
taskCNT.ci_channels.add_ci_ang_encoder_chan(counter = 'Dev4/ctr2', decoding_type = EncoderType.X_4, units=AngleUnits.DEGREES, pulses_per_rev=512, initial_angle=0.0)
POSWrist=[0 for i in range(LoopN)]
VELWrist=[0 for i in range(LoopN)]
OOWrist=[0 for i in range(LoopN)]
taskAO.start()
taskDO0.start()
taskDO1.start()
taskCNT.start()
VelWrist=0
PangWrist=0
TIME=[0 for i in range(LoopN)]
A=time.time()
for i in range(LoopN):
angWrist = taskCNT.read()
VelWrist=0.95*VelWrist+0.05*(angWrist-PangWrist)/10
PangWrist=angWrist
ooWrist=torque #torque for wrist = 0.2/10
if ooWrist>0:
if ooWrist>10:
ooWrist=10
taskAO.write(ooWrist)
taskDO0.write(True)
taskDO1.write(False)
else:
if ooWrist<-10:
ooWrist=-10
taskAO.write(-ooWrist)
taskDO1.write(True)
taskDO0.write(False)
POSWrist[i]=angWrist
VELWrist[i]=VelWrist
TIME[i]=time.time()-A
taskAO.stop()
taskDO0.stop()
taskDO1.stop()
taskCNT.stop()
MotorStop("Dev4")
"""
OOWrist=[-0.4*(POSWrist[i]-DangWrist)-310*VELWrist[i] for i in range(LoopN)]
plt.figure("Wrist")
plt.plot(TIME,OOWrist, "b")
plt.plot(TIME,POSWrist, "r")
plt.plot(TIME,VELWrist, "g")
"""
if __name__ == "__main__":
signal.signal(signal.SIGINT, handler)
main(LoopW=0, LoopT=10, LoopI=10)
print("Dark")
wrist(-0.2)
print("Light")
wrist(0.08)
print("Dark")
wrist(-0.2)
allStop("Dev4")
