forked from conorobrien/hapticsstb
-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathSTB_Servo_Squeeze.py~
169 lines (146 loc) · 4.96 KB
/
STB_Servo_Squeeze.py~
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
from __future__ import division
#!/usr/bin/env python
## BEFORE RUNNING ON NEW COMPUTER
# Install Phidget Libraries: http://www.phidgets.com/docs/OS_-_Linux#Getting_Started_with_Linux
# Install Phidget Python libraries: http://www.phidgets.com/docs/Language_-_Python#Linux
# Test Phidget with demo code: http://www.phidgets.com/downloads/examples/Python.zip
# Make sure it works with demo code, this code is pretty basic
# Phidget Python API reference: http://www.phidgets.com/documentation/web/PythonDoc/Phidgets.html
import csv
import time
import os
import datetime
import time
import numpy as np
import hapticsstb
import math
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Devices.AdvancedServo import AdvancedServo
#Do Not Make Force_Scale less than 1
FORCE_SCALE = 1
def Error(e):
try:
source = e.device
print("Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
# Setup Phidget
servo = AdvancedServo()
servo.setOnErrorhandler(Error)
# Open Phidget
servo.openPhidget()
servo.waitForAttach(500)
# Set Phidget servo parameters
try:
motor = 0
servo.setEngaged(0, True)
servo.setEngaged(1, True)
servo_min = servo.getPositionMin(motor) + 100
servo_max = servo.getPositionMin(motor) + 150
servo_mid = (servo_max - servo_min)/2
servo.setAcceleration(1, 500)
servo.setAcceleration(motor, 500) # I just picked these to be smooth, feel free to change
servo.setVelocityLimit(1, 2000)
servo.setVelocityLimit(motor, 2000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
# Set up STB
sample_rate = 50 # This works best with low sampling rates
# Call STB's constructer (init)
sensor = hapticsstb.STB(sample_rate)
sensor.bias() # Tares the sensor, make sure that nothing's touching the bordcr
print sensor.bias_vector
run_time = 500
volt = 3.3
start_time = time.time()
sensor.start_sampling() # Starts the STB hardware sampling loop
try:
fname = time.strftime("%H_%M_%S_%m_%d_%Y")
newdir = "/home/haptics/joshdata/"
os.chdir(newdir)
while time.time() - start_time <= run_time:
# read_M40 returns [Fx, Fy, Fz, Tx, Ty, Tz]
sensor_data = sensor.read_m40()
handedness = sensor.read_acc()
#print("left hand" + str((handedness[0])))
#print("right hand" + str((handedness[1])))
#print sensor_data[0]
#print sensor_data[1]
#print sensor_data[2]
mag = (sensor_data[0]**2 + sensor_data[1]**2 + sensor_data[2]**2) ** (1/2)
#print handedness[0],handedness[1],mag
conv = (5/7)
pos = servo_min + (mag * ((servo_max - servo_min)*conv) / FORCE_SCALE)
#pos = servo_max;
#eric = (pos, "left")
#josh = (pos, "right")
#hand one
if mag > 0.1:
if handedness[0] < volt:
# Scale force to +- 30N for whole range of servo
if pos <= servo_max and pos >= servo_min:
servo.setPosition(0, pos)
elif pos > servo_max:
servo.setPosition(0, servo_max)
elif pos < servo_min:
servo.setPosition(0, servo_min)
#hand two
if handedness[1] < volt:
if pos <= servo_max and pos >= servo_min:
servo.setPosition(1, pos)
elif pos > servo_max:
servo.setPosition(1, servo_max)
elif pos < servo_min:
servo.setPosition(1, servo_min)
#Both Hands
if handedness[0] > volt and handedness[1] > volt:
if pos <= servo_max and pos >= servo_min:
servo.setPosition(0, pos)
servo.setPosition(1, pos)
elif pos > servo_max:
servo.setPosition(0, servo_max)
servo.setPosition(1, servo_max)
elif pos < servo_min:
servo.setPosition(0, servo_min)
servo.setPosition(1, servo_min)
else:
servo.setPosition(1, servo_min)
servo.setPosition(0, servo_min)
cat = (pos, time.time()-start_time, sensor_data[0], sensor_data[1], sensor_data[2])
#sensor_data[0] is force in x
#sensor_data[1] is force in y
#sensor_data[2] is force in z
with open(fname + ".csv", "a") as csvfile:
spamwriter = csv.writer(csvfile, delimiter = ',')
spamwriter.writerow(cat)
# Scale force to +- 30N for whole range of servo
#pos = servo_mid + (sensor_data[2])*(servo_max - servo_mid)/FORCE_SCALE
#if pos <= servo_max and pos >= servo_min:
# servo.setPosition(motor, pos)
# servo.setPosition(1, pos)
#elif pos > servo_max:
# servo.setPosition(motor, servo_max)
# servo.setPosition(1, servo_max)
#
#elif pos < servo_min:
# servo.setPosition(motor, servo_min)
# servo.setPosition(1, servo_min)
# print( "Sensor data: " + str(sensor_data) );
except KeyboardInterrupt:
pass
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
except:
servo.setPosition(1, servo_min)
servo.setPosition(0, servo_min)
sensor.close() # Need to run this when you're done, stops STB and closes serial port
servo.closePhidget()
raise
servo.setPosition(1, servo_min)
servo.setPosition(0, servo_min)
sensor.close()
servo.closePhidget()