unityでleap motionを使う

前提・実現したいこと

arduinoとunityを連携してleap motionで表示した手とCubeが接触したときにLEDがつき、unityでHelloと表示される、逆に手とCubeが離れたときはLEDが消え、byeと表示されるようなものを作りたいと考えています。

発生している問題・エラーメッセージ

leap motionで手を表示した瞬間にHand modelとCubeが見た目では明らかに接触していないのに
Byeと表示され、LEDが消えたままになります。

該当のソースコード

ParentThumb.cs
1//ParentThumb.cs
2using UnityEngine;
3using System.Collections;
4using Uduino; // adding Uduino NameSpace
5
6
7public class ParentThumb : MonoBehaviour
8{
9    UduinoManager u; // The instance of Uduino is initialized here
10    public int blinkPin = 13;
11    [Range(0, 5)]
12    private bool isIndexTouch;
13    void Start()
14    {
15        UduinoManager.Instance.pinMode(blinkPin, PinMode.Output);    
16    }
17
18    void OnCollisionEnter(Collision collision)
19    {
20        if (collision.gameObject.name == "Cube")
21        {
22            isIndexTouch = true;
23        }
24    }
25
26    void OnCollisionStay(Collision collision)
27    {
28        if (collision.gameObject.name == "Cube")
29        {
30            isIndexTouch = true;
31        }
32    }
33
34    void OnCollisionExit(Collision collision)
35    {
36        if (collision.gameObject.name == "Cube")
37        {
38            isIndexTouch = false;
39        }
40    }
41
42    void Update()
43    {
44        if (isIndexTouch == true)
45        {
46            UduinoManager.Instance.digitalWrite(blinkPin, State.HIGH);
47            Debug.Log("Hello", gameObject);
48        }
49        else
50        {
51            UduinoManager.Instance.digitalWrite(blinkPin, State.LOW);
52            Debug.Log("Bye", gameObject);
53        }
54    }
55
56    
57}

uduino
1// Uduino Default Board
2#include<Uduino.h>
3Uduino uduino("uduinoBoard"); // Declare and name your object
4
5// Servo
6#include <Servo.h>
7#define MAXSERVOS 8
8
9
10void setup()
11{
12  Serial.begin(9600);
13
14#if defined (__AVR_ATmega32U4__) // Leonardo
15  while (!Serial) {}
16#elif defined(__PIC32MX__)
17  delay(1000);
18#endif
19
20  uduino.addCommand("s", SetMode);
21  uduino.addCommand("d", WritePinDigital);
22  uduino.addCommand("a", WritePinAnalog);
23  uduino.addCommand("rd", ReadDigitalPin);
24  uduino.addCommand("r", ReadAnalogPin);
25  uduino.addCommand("br", BundleReadPin);
26  uduino.addCommand("b", ReadBundle);
27  uduino.addInitFunction(DisconnectAllServos);
28  uduino.addDisconnectedFunction(DisconnectAllServos);
29}
30
31void ReadBundle() {
32  char *arg = NULL;
33  char *number = NULL;
34  number = uduino.next();
35  int len = 0;
36  if (number != NULL)
37    len = atoi(number);
38  for (int i = 0; i < len; i++) {
39    uduino.launchCommand(arg);
40  }
41}
42
43void SetMode() {
44  int pinToMap = 100; //100 is never reached
45  char *arg = NULL;
46  arg = uduino.next();
47  if (arg != NULL)
48  {
49    pinToMap = atoi(arg);
50  }
51  int type;
52  arg = uduino.next();
53  if (arg != NULL)
54  {
55    type = atoi(arg);
56    PinSetMode(pinToMap, type);
57  }
58}
59
60void PinSetMode(int pin, int type) {
61  //TODO : vérifier que ça, ça fonctionne
62  if (type != 4)
63    DisconnectServo(pin);
64
65  switch (type) {
66    case 0: // Output
67      pinMode(pin, OUTPUT);
68      break;
69    case 1: // PWM
70      pinMode(pin, OUTPUT);
71      break;
72    case 2: // Analog
73      pinMode(pin, INPUT);
74      break;
75    case 3: // Input_Pullup
76      pinMode(pin, INPUT_PULLUP);
77      break;
78    case 4: // Servo
79      SetupServo(pin);
80      break;
81  }
82}
83
84void WritePinAnalog() {
85  int pinToMap = 100;
86  char *arg = NULL;
87  arg = uduino.next();
88  if (arg != NULL)
89  {
90    pinToMap = atoi(arg);
91  }
92
93  int valueToWrite;
94  arg = uduino.next();
95  if (arg != NULL)
96  {
97    valueToWrite = atoi(arg);
98
99    if (ServoConnectedPin(pinToMap)) {
100      UpdateServo(pinToMap, valueToWrite);
101    } else {
102      analogWrite(pinToMap, valueToWrite);
103    }
104  }
105}
106
107void WritePinDigital() {
108  int pinToMap = -1;
109  char *arg = NULL;
110  arg = uduino.next();
111  if (arg != NULL)
112    pinToMap = atoi(arg);
113
114  int writeValue;
115  arg = uduino.next();
116  if (arg != NULL && pinToMap != -1)
117  {
118    writeValue = atoi(arg);
119    digitalWrite(pinToMap, writeValue);
120  }
121}
122
123void ReadAnalogPin() {
124  int pinToRead = -1;
125  char *arg = NULL;
126  arg = uduino.next();
127  if (arg != NULL)
128  {
129    pinToRead = atoi(arg);
130    if (pinToRead != -1)
131      printValue(pinToRead, analogRead(pinToRead));
132  }
133}
134
135void ReadDigitalPin() {
136  int pinToRead = -1;
137  char *arg = NULL;
138  arg = uduino.next();
139  if (arg != NULL)
140  {
141    pinToRead = atoi(arg);
142  }
143
144  if (pinToRead != -1)
145    printValue(pinToRead, digitalRead(pinToRead));
146}
147
148void BundleReadPin() {
149  int pinToRead = -1;
150  char *arg = NULL;
151  arg = uduino.next();
152  if (arg != NULL)
153  {
154    pinToRead = atoi(arg);
155    if (pinToRead != -1) {
156      if (pinToRead < 13)
157        printValue(pinToRead, digitalRead(pinToRead));
158      else
159        printValue(pinToRead, analogRead(pinToRead));
160    }
161  }
162}
163
164Servo myservo;
165void loop()
166{
167  uduino.update();
168}
169
170void printValue(int pin, int targetValue) {
171  uduino.print(pin);
172  uduino.print(" "); //<- Todo : Change that with Uduino delimiter
173  uduino.println(targetValue);
174  // TODO : Here we could put bundle read multiple pins if(Bundle) { ... add delimiter // } ...
175}
176
177
178
179
180/* SERVO CODE */
181Servo servos[MAXSERVOS];
182int servoPinMap[MAXSERVOS];
183/*
184  void InitializeServos() {
185  for (int i = 0; i < MAXSERVOS - 1; i++ ) {
186    servoPinMap[i] = -1;
187    servos[i].detach();
188  }
189  }
190*/
191void SetupServo(int pin) {
192  if (ServoConnectedPin(pin))
193    return;
194
195  int nextIndex = GetAvailableIndexByPin(-1);
196  if (nextIndex == -1)
197    nextIndex = 0;
198  servos[nextIndex].attach(pin);
199  servoPinMap[nextIndex] = pin;
200}
201
202
203void DisconnectServo(int pin) {
204  servos[GetAvailableIndexByPin(pin)].detach();
205  servoPinMap[GetAvailableIndexByPin(pin)] = 0;
206}
207
208bool ServoConnectedPin(int pin) {
209  if (GetAvailableIndexByPin(pin) == -1) return false;
210  else return true;
211}
212
213int GetAvailableIndexByPin(int pin) {
214  for (int i = 0; i < MAXSERVOS - 1; i++ ) {
215    if (servoPinMap[i] == pin) {
216      return i;
217    } else if (pin == -1 && servoPinMap[i] < 0) {
218      return i; // return the first available index
219    }
220  }
221  return -1;
222}
223
224void UpdateServo(int pin, int targetValue) {
225  int index = GetAvailableIndexByPin(pin);
226  servos[index].write(targetValue);
227  delay(10);
228}
229
230void DisconnectAllServos() {
231  for (int i = 0; i < MAXSERVOS; i++) {
232    servos[i].detach();
233    digitalWrite(servoPinMap[i], LOW);
234    servoPinMap[i] = -1;
235  }
236}