Sorting by colour with MeArm and TCS34725

// Servo 0: base rotation
// Servo 1: direct in/out movement
// Servo 2: in/out movement via linkage
// Servo 3: gripper

// Servo pins
var SERVOS = [C4,B15,B14,B13];
// Servo home pulse widths - tweak these for different arms (depending on assembly)
var SERVOHOME = [1.51,1.5,1.5,1.5];
// gripper positions
var JOPEN = 0;
var JCLOSE = 0.45;

// Keyframes for movement of arm. Each line
// is a position, and we move between them
// at 2fps

// Pick up token, move it to the sensor
var KF_PICKUP = [
  [ 0, 0, 0, JOPEN ], // default position
 // [ 0, 0.3, -0.1, JOPEN ],
  [ 0, 0.25, -0.1, JOPEN ],
  [ 0, 0.45, -0.3, JOPEN ], // over token here
  [ 0, 0.45, -0.3, JCLOSE ], // over token here
  [ 0, 0.25, -0.1, JCLOSE ],
  [ 0, 0, 0, JCLOSE ],
  [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
  [ 0.75, -0.3, 0, JCLOSE ],
];
// Hold steady at the sensor
var KF_TEST_RGB = [ // just hold steady for 2 secs
  [ 0.75, -0.3, 0, JCLOSE ],
  [ 0.75, -0.3, 0, JCLOSE ],
  [ 0.75, -0.3, 0, JCLOSE ],
  [ 0.75, -0.3, 0, JCLOSE ],
  [ 0.75, -0.3, 0, JCLOSE ],
];
// move from the sensor to a location, release gripper, go home
var KF_DOWN_LEFT = [
  [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
  [ 0.3, 0.45, -0.3, JCLOSE ], // down left
  [ 0.3, 0.45, -0.3, JOPEN ],
  [ 0, 0, 0, JOPEN ], // home
  [ 0, 0, 0, JOPEN ],
];
var KF_DOWN_RIGHT = [
  [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
  [ -0.3, -0.3, 0, JCLOSE ],
  [ -0.3, 0.7, 0.0, JCLOSE ], // down left
  [ -0.3, 0.7, 0.0, JOPEN ],
  [ 0, 0, 0, JOPEN ], // home
  [ 0, 0, 0, JOPEN ],
];
var KF_DOWN_RIGHT_NEAR = [
  [ 0.75, -0.3, 0, JCLOSE ], // colour sensor here
  [ -0.4, -0.3, 0, JCLOSE ],
  [ -0.4, -0.2, -0.4, JCLOSE ],
  [ -0.4, -0.2, -0.4, JOPEN ], // down left
  [ 0, 0, 0, JOPEN ], // home
  [ 0, 0, 0, JOPEN ],
];

// variables for keyframe movement
var keyInterval;
var keyPos = 0;
var keyEnd = 0;
var keyFrames = [];
var keyCallback;


var rgbAvr = [0,0,0]; // average RGB value
var tcs; // rgb sensor
var isRunning = false; // for on/off button
var ledRGBs = new Uint8Array(25*3); // values for LED strip

// Called to animate between keyframes
function step() {
  keyPos += 0.04; // 20ms * 2fps
  var n = 0|keyPos;
  var a = keyPos-n;
  var n2 = n+1;
  //print(keyPos,n,n2,a);
  for (var i in SERVOS) {
    // interpolate between keyframes
    var p = keyFrames[n][i]*(1-a) + keyFrames[n2][i]*a;
    // move servo
    digitalPulse(SERVOS[i], 1, SERVOHOME[i]+p);
  }
  // are we finished?
  if (keyPos >= keyEnd) {
    clearInterval(keyInterval);
    keyInterval = undefined;
    if (keyCallback) keyCallback();
  }
}

// Play back an array of keyframes, call callback when done
function playback(frames, callback) {
  keyPos = 0 - 0.02;
  keyEnd = frames.length-2;
  keyFrames = frames;
  keyCallback = callback;
  keyInterval = setInterval(step, 20);
}

// Test a single token
function testToken() {
  // pick it up
  playback(KF_PICKUP, function() {
    // then wait with it by the sensor for 2 secs
    playback(KF_TEST_RGB, function() {
      // print the colour values and work out if it's red, green or blue      
      // finally put it down in the right place and start again
      print(rgbAvr);
      if (rgbAvr[0] > rgbAvr[1] && rgbAvr[0] > rgbAvr[2]) { // red
        print("Red!");        
        playback(KF_DOWN_LEFT, testToken);
      } else if (rgbAvr[1] > rgbAvr[0] && rgbAvr[1] > rgbAvr[2]) { // green
        print("Green!");
        playback(KF_DOWN_RIGHT, testToken);
      } else {
        print("Blue!");
        playback(KF_DOWN_RIGHT_NEAR, testToken); // blue
      }
    });
  });
}

// Get RGB value from the sensor
function getRGB() {
  // get the value
  var v = tcs.getValue();
  // re-scale it, to get full saturation and lightness (leaving just hue)
  var min = Math.min(v.red, v.green, v.blue);
  var range = Math.max(v.red, v.green, v.blue) - min;
  if (range<1) range=1;
  var rgb = [ // values between 0 and 255
    (v.red - min)*200/range, // tweak red - so we don't accidentally classify green sometimes
    (v.green - min)*255/range,
    (v.blue - min)*255/range,
  ];
  // handle the average
  rgbAvr[0] = rgbAvr[0]*0.9 + rgb[0]*0.1;
  rgbAvr[1] = rgbAvr[1]*0.9 + rgb[1]*0.1;
  rgbAvr[2] = rgbAvr[2]*0.9 + rgb[2]*0.1;
  // set the LED strip to the right colour
  for (var i=0;i<ledRGBs.length;i+=3)
    ledRGBs.set(rgbAvr,i);
  SPI1.send4bit(ledRGBs, 0b0001, 0b0011);
}

// Start the arm working - initialise RGB sensor and start movement
function startArm() {
  SPI1.setup({baud:3200000, mosi:B5}); // LEDs
  I2C1.setup({scl:B8, sda:B9}); // RGB sensor
  tcs = require("TCS3472x").connect(I2C1, 
    1 /*integration cycles*/, 
    1 /*gain*/);
  testToken(); // start arm movement
  setTimeout(function() {
    // we seem to need a delay after initialisation before we use the RGB sensor
    setInterval(getRGB,20);
  },1000);
}

// On/off button
setWatch(function() {
  if (!isRunning) {
    LED1.set();
    isRunning = true;
    startArm();
  } else {
    LED1.reset();
    isRunning = false;
    clearInterval();
  }
}, BTN, {edge:"rising", repeat:true, debounce:50});
    

// For testing - call startTest() and then change the
// values in the test array to work out the positions
// needed for the keyframes array
var test = [0,0,0,0];
function startTest() {
  setInterval(function() {
    for (var i in SERVOS) {
      digitalPulse(SERVOS[i], 1, SERVOHOME[i]+test[i]);
    }
  }, 20);
}
//startTest()