Last active
March 10, 2024 02:36
-
-
Save zalo/2332575f3a7b45dedd607016cb3e90ae to your computer and use it in GitHub Desktop.
16 Servo Driver - https://youtu.be/KsYlQIv7wcw
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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); | |
} | |
} | |
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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); | |
} | |
} | |
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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