Skip to content

Instantly share code, notes, and snippets.

@zalo
Last active March 10, 2024 02:36
Show Gist options
  • Save zalo/2332575f3a7b45dedd607016cb3e90ae to your computer and use it in GitHub Desktop.
Save zalo/2332575f3a7b45dedd607016cb3e90ae to your computer and use it in GitHub Desktop.
16 Servo Driver - https://youtu.be/KsYlQIv7wcw
// This is just an example script for converting hand motions to PWM Servo Angles 1-255
// Designed to be used with UnityModules - https://github.com/leapmotion/UnityModules
using Leap;
using Leap.Unity;
using UnityEngine;
public class HandAngleParser : MonoBehaviour {
private const int N_FINGERS = 5;
private const int N_ACTIVE_BONES = 2;
public LeapServiceProvider serviceProvider;
public ThreadedSerialDataWriter dataWriter;
[Range(1, 255)]
public int motorPos = 1;
void Update() {
bool rightFound = false;
foreach(Hand hand_ in serviceProvider.CurrentFrame.Hands) {
if (hand_.IsRight) {
rightFound = true;
byte[] angles = new byte[17];
angles[16] = 0;
int curIndex = 0;
// Iterate through the bones in the hand, applying drive forces
for (int fingerIndex = 0; fingerIndex < N_FINGERS; fingerIndex++) {
for (int jointIndex = 0; jointIndex < N_ACTIVE_BONES; jointIndex++) {
Bone prevBone = hand_.Fingers[fingerIndex].Bone((Bone.BoneType)(jointIndex));
Bone bone = hand_.Fingers[fingerIndex].Bone((Bone.BoneType)(jointIndex + 1));
float xTargetAngle = Vector3.SignedAngle(
prevBone.Rotation.ToQuaternion() * ((fingerIndex == 0 && jointIndex == 0) ? -Vector3.up : Vector3.forward),
bone.Direction.ToVector3(),
prevBone.Rotation.ToQuaternion() * Vector3.right);
xTargetAngle = (fingerIndex == 0 && jointIndex == 0) ? xTargetAngle + 120f : xTargetAngle;
if (jointIndex == 0) {
float yTargetAngle = Vector3.SignedAngle(
prevBone.Rotation.ToQuaternion() * Vector3.right,
bone.Rotation.ToQuaternion() * Vector3.right,
prevBone.Rotation.ToQuaternion() * Vector3.up);
angles[curIndex++] = (byte) ((Mathf.Clamp01(yTargetAngle / 90f) * 254) + 1);
}
angles[curIndex++] = (byte)((Mathf.Clamp01(xTargetAngle / 90f) * 254) + 1);
}
}
dataWriter.WriteBytes(angles);
}
}
if (!rightFound) {
byte[] angles = new byte[17];
angles[16] = 0;
for (int i = 0; i < 16; i++) {
angles[i] = (byte)motorPos;
}
dataWriter.WriteBytes(angles);
}
}
}
using System;
using System.Text;
using System.IO.Ports;
using UnityEngine;
using Leap.Unity;
public class ThreadedSerialDataWriter : MonoBehaviour {
//Default serial port name; unused since we just grab the last COM port anyway
string portName = "COM3";
SerialPort _serialPort;
//Define our thread-safe Queue for getting data from the COM port reading thread to the main thread
ProduceConsumeBuffer<byte[]> SerialData = new ProduceConsumeBuffer<byte[]>(128);
//Misc. Utility Objects
Action kickoffWrite = null;
void Start() {
// Defaults to last port in list (most chance to be our controller receiver's port)
string[] portNames = SerialPort.GetPortNames();
portName = portNames[portNames.Length - 1];
//Have to add the prefix on Windows; probably won't work on any other platform
Debug.Log(@"\\.\" + portName);
_serialPort = new SerialPort(@"\\.\" + portName, 115200);
_serialPort.ReadTimeout = 1;
_serialPort.WriteTimeout = 1;
_serialPort.DataBits = 8;
_serialPort.Parity = Parity.None;
_serialPort.StopBits = StopBits.One;
_serialPort.Open();
if (_serialPort.IsOpen) {
Debug.Log("Opened " + portName);
//Flush the data that is currently sitting in the Serial Port
_serialPort.DiscardInBuffer();
//Begin writing to the serial port asynchronously in another thread
kickoffWrite = delegate {
while (_serialPort.IsOpen) {
byte[] toWrite;
if(SerialData.TryDequeue(out toWrite)) {
_serialPort.Write(toWrite, 0, toWrite.Length);
}
}
};
kickoffWrite.BeginInvoke(null, null);
}
}
/// <summary>Bytes range from 1-255; ensure that there is always a 0 at the end to delineate packets. </summary>
public void WriteBytes(byte[] bytes) {
SerialData.TryEnqueue(bytes);
}
//Kill our COM port thread and our COM port
void OnApplicationQuit() {
try {
kickoffWrite.EndInvoke(null);
} catch (System.Runtime.Remoting.RemotingException exc) {
Debug.Log("Serial Polling Thread Shutting down\n" + exc.ToString());
} catch (Exception exc) {
Debug.Log(exc.ToString());
}
_serialPort.Close();
if (!_serialPort.IsOpen) {
Debug.Log("Closed " + portName);
} else {
Debug.Log("Couldn't close " + portName);
}
}
}
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 425 // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
void setup() {
Serial.begin(115200);
Serial.println("16 channel Servo test!");
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
delay(10);
}
uint8_t index = 0; // The next ServoPosition Index to write to.
uint8_t servoPositions[16] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
void loop() {
// Parse the SerialPort buffer
// 0 is a sentinel value that returns it to the beginning
while (Serial.available() > 0) {
// read the incoming bytes:
uint8_t incomingByte = Serial.read();
if (incomingByte > 0){
servoPositions[index++] = incomingByte;
} else {
index = 0;
}
}
// Set all 16 Servos to their respective Positions
for (uint8_t servonum = 0; servonum < 16; servonum++) {
pwm.setPWM(servonum, 0, map(servoPositions[servonum], 1, 255, SERVOMIN, SERVOMAX));
}
delay(16); // Wait 16ms
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment