Kinect Controller for Helicopter FS-IRH100

RC-heli.ino

// Arduino Sketch to send IR signal to FS-IRH100

// Connect Infra-red LED to pin 3 & GND

// bit set / clear
#ifndef cbi
#define cbi(PORT, BIT) (_SFR_BYTE(PORT) &= ~_BV(BIT))
#endif
#ifndef sbi
#define sbi(PORT, BIT) (_SFR_BYTE(PORT) |= _BV(BIT))
#endif

static byte data[8];

int popl(byte b) {
  b = (b&0x55) + (b>>1&0x55);
  b = (b&0x33) + (b>>2&0x33);
  return (b&0xf) + (b>>4&0xf);
}

byte parity(byte b) {
  return popl(b) & 1;
}

void setup() {
  Serial.begin(115200);
  
  pinMode(3, OUTPUT); // set OC2B (pin 3) to OUTPUT mode
  digitalWrite(3, LOW);
  TCCR2A = _BV(WGM20);
  TCCR2B = _BV(WGM22) | _BV(CS20);
  OCR2A = F_CPU / 2 / 38000/*hz*/;
  OCR2B = OCR2A / 2; /* 50% duty */
}

void send_signal(int off_us, int on_us) {
  delayMicroseconds(off_us);
  sbi(TCCR2A,COM2B1); /* PWM start */
  delayMicroseconds(on_us);
  cbi(TCCR2A,COM2B1); /* end */
}

void send_data() {
  for (int i = 0; i < 8; i++) {
    Serial.print(data[i], HEX);
  }
  Serial.print("\n");
  send_signal(0, 3180); // leader
  for (int i = 0; i < 8; i++) {
    for (int j = 0; j < 8; j+=2) {
      send_signal(data[i] & (1 << j) ? 1030 : 510,
                  data[i] & (2 << j) ? 1030 : 510);
    }
  }
}

// up   - 00:stop  - 0x64
// lr   - 0f<-01:left 10:n 11->1f:right
// trim - 0f<-00:left 10:n 11->1f:right
// fb   -  1<- 7:back  8:n  9->f :front
// ls,rs- true/false
void generate_data(byte up, byte lr, byte trim, byte fb, bool ls, bool rs) {
  byte chk = (up + lr + trim/2 + fb*0x11 + ((ls||rs)?0x10:0x00)) & 0x1f;
  
  data[0] = up;
  data[1] = parity(up);
  data[1] |= lr << 1;
  data[2] = 0x01;
  data[2] |= (1-parity(lr)) << 1;
  data[2] |= (trim >> 1) << 2;
  data[2] |= (fb & 0x3) << 6;
  data[3] = (fb & 0xc) >> 2;
  data[3] |= (parity(fb) ^ parity(trim >> 1)) << 2;
  data[3] |= chk << 3;
  data[4] = (lr > 0x20 ? 1 : 0) << 2;
  data[4] |= parity(chk) << 3;
  data[4] |= fb << 4;
  data[5] = ls ? 0x7 : rs ? 0xf : 0x8;
  data[5] |= (byte[]){0,0,0,3,2,3,3,2,4,1,7,6,7,6,6,7}[fb] << 4;
  data[6] = lr != 0x10 ? 0x60 : fb != 0x8 ? 0x64 : 0x44;
  data[7] = ls ? 0x5d : 0x0d;

  if (rs) data[5] ^= 0x10;
}

byte read_byte() {
  while (!Serial.available());
  return Serial.read();
}

byte read_hex() {
  byte x, i = 2, ret = 0;
  while (i--) {
    ret <<= 4;
    x = read_byte();
    if (x >= '0' && x <= '9')
      ret |= x - '0';
    else if (x >= 'a' && x <= 'f')
      ret |= x - 'a' + 10;
    else if (x >= 'A' && x <= 'F')
      ret |= x - 'A' + 10;
    else
      return 0xff;
  }
  return ret;
}

void loop() {
  byte up, lr, trim, fb, sh;
  
  Serial.print("up:");
  up = read_hex();
  if (up == 0xff) return;
  Serial.println(up, HEX);

  Serial.print("lr:");
  lr = read_hex();
  if (lr == 0xff) return;
  Serial.println(lr, HEX);

  Serial.print("trim:");
  trim = read_hex();
  if (trim == 0xff) return;
  Serial.println(trim, HEX);

  Serial.print("fb:");
  fb = read_hex();
  if (fb == 0xff) return;
  Serial.println(fb, HEX);

  Serial.print("sh:");
  sh = read_byte();
  if (sh == 0xff) return;
  Serial.println(sh=='l'?"l":sh=='rs'?"r":"n");
  
  generate_data(up, lr, trim, fb, sh=='l', sh=='r');
  send_data();
}

RC-heli.pde

int LR_TRIM = 16;  // adjust this between 1-31 if the helicopter always rotates left or right

import SimpleOpenNI.*;
import processing.serial.*;
Serial serial;

SimpleOpenNI  context;
boolean       autoCalib=true;

void setup()
{
  serial = new Serial(this,"/dev/tty.usbserial-A7006UFY", 115200);
  context = new SimpleOpenNI(this);
   
  // enable depthMap generation 
  if(context.enableDepth() == false)
  {
     println("Can't open the depthMap, maybe the camera is not connected!"); 
     exit();
     return;
  }
  
  // enable skeleton generation for all joints
  context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
 
  background(200,0,0);

  stroke(0,0,255);
  strokeWeight(3);
  smooth();
  
  size(context.depthWidth(), context.depthHeight()); 
}

void draw()
{
  // update the cam
  context.update();
  
  // draw depthImageMap
  image(context.depthImage(),0,0);
  
  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  for(int i=0;i<userList.length;i++)
  {
    if(context.isTrackingSkeleton(userList[i])) {
      drawSkeleton(userList[i]);
      sendCommand(userList[i]);
    }
  }    
}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  // to get the 3d joint data
  /*
  PVector jointPos = new PVector();
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,jointPos);
  println(jointPos);
  */
  
  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);  
}

int last = millis();
void sendCommand(int userId)
{
  if (millis() - last < 200) return;
  last = millis();
  
  PVector left = new PVector(), right = new PVector();
  PVector hip_l = new PVector(), hip_r = new PVector(), neck = new PVector();
  PVector shoulder_l = new PVector(), torso = new PVector();
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND, left);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HAND, right);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER, shoulder_l);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_TORSO, torso);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HIP, hip_l);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HIP, hip_r);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,  neck);

  if (neck.y < 0)
    return;

  double ry = (right.y - hip_r.y)  / (neck.y - hip_r.y) * 32;
  double ly = (left.y - hip_l.y) / (neck.y - hip_l.y) * 32;
  double lx = (left.x - shoulder_l.x) / (shoulder_l.x - torso.x) * 100;
  double lz = (left.z - shoulder_l.z) / (shoulder_l.x - torso.x) * 100;
  
  int up = (int)(ry < 0 ? 0 : ry < 50 ? ry+32 : ry < 59 ? 82+(ry-50)*2 : 100);
  int lr = 16;
  int trim = LR_TRIM;
  int fb = 8;
  char slide = 'n';

  if (up > 0) {
    if (ly > 15 && ly < 32) {
      if (lx >  300) lr = 24;
      if (lx < -200) lr = 12;
      if (lz >  250) fb = 14;
      if (lz < -150) fb =  6;
    }
  }  
  serial.write(hex(up,2) + hex(lr,2) + hex(trim,2) + hex(fb,2) + slide + "\n");
  while(serial.available() > 0) {
    print(serial.readString());
  }
}

// -----------------------------------------------------------------
// SimpleOpenNI events

void onNewUser(int userId)
{
  println("onNewUser - userId: " + userId);
  println("  start pose detection");
  
  if(autoCalib)
    context.requestCalibrationSkeleton(userId,true);
  else    
    context.startPoseDetection("Psi",userId);
}

void onLostUser(int userId)
{
  println("onLostUser - userId: " + userId);
}

void onExitUser(int userId)
{
  println("onExitUser - userId: " + userId);
}

void onReEnterUser(int userId)
{
  println("onReEnterUser - userId: " + userId);
}

void onStartCalibration(int userId)
{
  println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
  
  if (successfull) 
  { 
    println("  User calibrated !!!");
    context.startTrackingSkeleton(userId); 
  } 
  else 
  { 
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    context.startPoseDetection("Psi",userId);
  }
}

void onStartPose(String pose,int userId)
{
  println("onStartPose - userId: " + userId + ", pose: " + pose);
  println(" stop pose detection");
  
  context.stopPoseDetection(userId); 
  context.requestCalibrationSkeleton(userId, true);
 
}

void onEndPose(String pose,int userId)
{
  println("onEndPose - userId: " + userId + ", pose: " + pose);
}