HapticHand/main.py at main · Patronics/HapticHand · GitHub

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
import time
import board
import busio
import digitalio
import analogio
import pwmio
import adafruit_vl53l0x
import math

import adafruit_drv2605


maxDistVal = 1270 #max recorded valid value so far
minPotVal = 300
maxPotVal = 65535


i2c = busio.I2C(board.GP3, board.GP2)
distSense = adafruit_vl53l0x.VL53L0X(i2c)

led = digitalio.DigitalInOut(board.LED)
led.direction = digitalio.Direction.OUTPUT

pot = analogio.AnalogIn(board.GP26_A0)

#motorPin = pwmio.PWMOut(board.GP16)
drv = adafruit_drv2605.DRV2605(i2c)

drv.realtime_value = 0
drv.mode = adafruit_drv2605.MODE_REALTIME

while True:
	led.value = True
	time.sleep(0.1)
	led.value = False
	time.sleep(0.1)
	print('Range: {}mm'.format(distSense.range))


	# distSense is ALWAYS 0 to 8192
	distSenseRange = distSense.range
	potVal = pot.value
	if (potVal < minPotVal):
		potVal = 0
	else:
		potVal = potVal/maxPotVal

	if (distSenseRange > (maxDistVal*potVal)):
		distSenseRange = maxDistVal

	print('Pot: {}'.format(potVal))
	normDist = distSenseRange / maxDistVal
	#dutyCycle = (1 - normDist) * 65535
	dutyCycle = (1 - normDist) * 127

	#motorPin.duty_cycle = math.floor(dutyCycle)
	# -128 through 127
	drv.realtime_value = int(dutyCycle)

	print('Duty Cycle: {}'.format(dutyCycle))