servo7.ino

#include <Servo.h>

/////////////////////////////////////
/// General setup for the Arduino ///
/////////////////////////////////////

char sCmd; // Data received from the serial port

// Sets up the pin connections on Arduino
// The numbers has to much to the ones the jumpers are connected to
int Xpin = 11;
int Ypin = 10;
int TriggerPin = 13; // Set the pin to digital I/O 13

// Sets up the starting point of the two servo motors
// These numbers need to be aligned before scanning so that the pinhole can be seen at the middle of each mirror (i.e. middle of the scanned scene)
float Xoffset = 6.0;
float Yoffset = -9.0;

Servo XServo;
Servo YServo;


////////////////////////////
/// Scanning path arrays ///
////////////////////////////

int sampling_time_ms = 260; // The amount of time between movements of the servo motors
int move_ms = 0;
int trigger_ms = 20;

int scanmode = 0; // default 0 - square 100 px
int nScanmodes = 5;


// The scanning path (servo positions) for 100 point square scan
const int8_t square100X[] PROGMEM = {    // 
  60,60,60,60,60,60,60,60,60,60,66,66,66,66,66,66,66,66,66,66,72,72,72,72,72,72,72,72,72,72,78,78,78,78,78,78,78,78,78,78,84,84,84,84,84,84,84,84,84,84,90,90,90,90,90,90,90,90,90,90,96,96,96,96,96,96,96,
  96,96,96,102,102,102,102,102,102,102,102,102,102,108,108,108,108,108,108,108,108,108,108,114,114,114,114,114,114,114,114,114,114,
  };
const int8_t square100Xdec[] PROGMEM = {    // 
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
  };
const int8_t square100Y[] PROGMEM = {    // 
  60,66,72,78,84,90,96,102,108,114,114,108,102,96,90,84,78,72,66,60,60,66,72,78,84,90,96,102,108,114,114,108,102,96,90,84,78,72,66,60,60,66,72,78,84,90,96,102,108,114,114,108,102,96,90,84,78,72,66,60,60,
  66,72,78,84,90,96,102,108,114,114,108,102,96,90,84,78,72,66,60,60,66,72,78,84,90,96,102,108,114,114,108,102,96,90,84,78,72,66,60,
  };
const int8_t square100Ydec[] PROGMEM = {    // 
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
  };


// The scanning path (servo positions) for 300 point spiral scan
const int8_t spiral300X[] PROGMEM = {    // 
  106,106,104,102,97,94,91,93,96,98,100,106,108,109,115,117,117,112,112,111,113,114,117,115,112,109,109,107,109,106,103,100,95,93,90,91,94,95,98,100,102,103,103,100,99,97,94,97,96,99,102,105,108,109,107
,104,101,98,98,101,106,103,100,98,95,97,95,92,91,89,86,90,88,91,93,96,93,91,88,88,89,91,88,88,86,86,85,80,77,78,83,85,85,84,80,81,81,79,75,74,74,77,80,83,87,89,86,83,81,78,77,75,72,71,70,70,67,70,73,74
,77,76,79,83,83,83,80,74,71,70,67,66,68,67,67,64,65,61,62,64,61,62,65,69,74,77,80,82,85,79,75,72,68,70,72,73,77,79,81,84,85,82,78,75,74,71,66,63,61,61,64,66,69,68,68,65,63,63,66,67,69,72,75,77,77,74,
71,70,69,72,73,75,74,77,77,80,76,78,80,81,84,86,87,86,90,90,89,91,90,89,87,85,82,80,77,80,83,82,85,83,81,85,84,87,87,88,90,91,92,95,95,93,94,93,95,92,90,88,86,89,92,93,95,93,97,99,103,101,99,99,96,96,
98,102,104,102,102,105,108,109,109,108,105,106,105,102,99,96,98,101,103,108,112,112,115,115,111,110,111,113,109,104,98,101,109,113,114,117,116,114,118,119,116,114,111,110,104,106,101,106,
  };

const int8_t spiral300Xdec[] PROGMEM = {    // 
  9,2,8,8,9,6,8,3,1,0,1,3,1,3,2,4,8,7,2,1,9,10,6,4,7,9,9,0,4,5,5,5,7,5,5,7,2,3,3,8,7,7,9,7,10,1,4,5,10,6,4,3,2,4,0,1,2,4,5,2,4,5,8,1,8,8,1,6,4,0,10,4,5,4,4,0,8,9,5,0,7,7,9,7,3,5,8,3,4,3,3,2,3,1,8,9,1,5,
6,6,0,1,3,3,1,7,1,2,1,2,3,4,6,4,8,7,5,10,9,3,1,9,9,3,0,9,0,4,9,0,8,3,7,9,5,5,2,10,9,2,1,7,9,8,4,7,8,9,2,4,3,2,0,5,2,5,0,4,7,8,0,1,1,5,7,6,1,7,1,6,6,8,3,7,6,9,4,7,4,5,2,1,0,10,7,5,5,4,2,1,2,0,5,4,9,6,1
,10,10,1,1,9,9,6,0,2,1,8,9,5,6,4,7,1,4,2,6,9,1,1,7,4,3,5,2,9,1,8,3,2,2,10,1,5,4,2,6,5,2,8,0,8,8,9,4,7,8,2,5,8,8,4,7,2,10,8,8,9,9,3,2,7,7,1,2,3,4,6,1,0,9,1,4,6,6,2,8,7,3,1,1,5,6,7,1,0,3,6,5,10,2,4,4,4,
10,8,1,10,3,6,
  };

const int8_t spiral300Y[] PROGMEM = {    // 
  98,95,92,90,91,91,92,87,88,86,89,89,91,94,95,93,97,100,103,107,106,102,101,98,96,97,101,101,104,104,103,102,106,103,102,99,97,94,94,92,95,98,101,99,97,97,100,100,103,105,106,107,107,110,114,113,116,115
,112,113,110,110,109,108,114,118,117,116,119,117,115,114,112,110,112,110,108,106,104,100,97,95,94,91,95,98,102,104,110,113,115,118,113,110,110,113,118,115,115,112,109,107,107,107,109,107,106,104,101,
101,104,100,99,102,105,108,109,111,106,103,98,94,95,94,97,100,98,97,95,97,95,97,100,103,106,103,100,100,97,94,93,91,92,93,93,91,92,90,91,89,89,91,90,88,84,87,87,84,87,86,84,83,82,84,81,81,83,85,83,87,
89,87,85,82,79,80,81,77,76,72,75,75,75,76,79,78,78,72,69,69,72,69,65,66,69,70,72,73,78,80,80,81,85,89,90,87,83,77,73,70,68,66,68,67,63,63,61,64,70,72,75,74,77,78,72,75,79,81,85,84,81,79,75,72,68,69,66
,64,62,61,63,60,62,65,65,62,63,65,70,73,74,71,67,68,66,70,73,74,76,73,70,67,68,71,77,76,76,77,80,80,80,79,75,72,75,78,78,80,85,88,87,84,83,83,83,83,80,81,83,86,87,91,89,91,93,90,87,85,86,81,
  };

const int8_t spiral300Ydec[] PROGMEM = {    // 
  2,3,8,7,1,7,4,9,7,3,2,5,9,6,2,4,3,2,8,4,6,8,3,9,7,7,0,3,4,5,10,9,4,6,6,7,4,5,3,10,4,1,0,10,1,8,8,9,7,7,9,6,7,9,2,9,6,7,2,3,9,6,7,3,4,9,7,2,3,5,4,3,1,6,7,7,7,0,8,8,5,5,1,2,3,3,9,7,5,2,9,2,0,4,7,4,3,9,2
,4,6,6,8,4,5,10,4,1,3,5,6,9,6,4,2,1,0,0,7,8,0,6,2,6,5,4,3,0,4,8,4,9,4,2,0,4,6,3,5,9,9,4,5,2,3,5,4,3,10,0,9,3,5,8,9,6,0,10,4,10,1,9,3,5,7,9,10,6,7,7,2,4,9,7,3,4,8,9,7,9,9,5,7,5,4,8,8,4,3,0,3,2,9,3,8,9,
7,6,0,9,6,4,9,4,0,6,3,0,5,6,3,4,5,2,1,8,6,9,2,6,2,9,6,1,6,6,6,9,0,8,9,2,3,0,3,2,8,7,10,6,5,7,9,8,3,9,5,4,7,7,2,1,9,1,2,10,9,8,2,3,4,5,10,9,7,10,10,7,5,1,5,0,4,5,1,0,2,9,8,6,8,2,5,4,5,4,8,2,9,4,4,2,7,7
,4,4,7,7,5,7,
  };


// The scanning path (servo positions) for 600 point spiral scan
const int8_t spiral600X[] PROGMEM = {    // 
  90,92,94,97,100,102,103,105,107,110,111,113,112,116,118,116,115,112,111,109,110,108,106,108,108,109,111,109,107,104,102,101,103,100,98,104,107,106,109,107,106,106,104,105,107,110,115,117,119,119,117,115
,114,114,112,113,116,116,114,111,112,115,113,113,110,111,111,108,106,105,103,102,99,99,101,101,92,91,93,99,101,104,106,105,108,106,102,99,97,97,97,98,97,95,93,93,95,95,95,92,90,94,96,99,103,103,101,
100,101,98,99,102,97,92,90,97,97,97,99,99,101,104,104,97,95,93,90,93,92,96,90,91,94,97,94,96,92,94,94,91,87,89,88,89,92,95,95,94,91,90,88,88,89,91,96,95,87,89,87,83,85,86,83,81,80,81,80,78,78,77,78,80
,81,81,80,81,83,85,83,83,85,83,83,85,86,85,86,85,87,85,87,89,88,90,88,87,85,87,86,84,82,79,77,79,78,78,79,80,82,85,85,83,81,78,76,74,70,68,68,66,65,64,66,69,70,73,73,75,77,82,80,76,74,75,75,76,76,74,71
  ,72,74,72,71,70,67,67,69,69,71,76,77,82,87,89,89,87,86,84,83,82,78,75,74,72,71,70,70,69,67,65,63,65,63,63,61,61,62,64,64,66,68,68,72,72,74,74,75,73,66,65,63,60,61,64,68,74,76,77,77,78,80,81,79,78,73,
69,67,66,65,63,62,60,61,62,61,63,63,65,66,68,65,66,67,69,70,72,75,77,81,79,76,79,77,73,70,68,66,64,64,67,68,71,70,72,74,75,72,71,74,73,71,69,69,71,72,72,70,69,67,68,65,65,69,73,74,76,79,79,80,81,82,84
,86,86,84,85,83,83,81,80,78,74,74,76,76,77,76,75,74,76,77,74,72,72,70,72,75,77,78,81,79,78,81,83,83,84,86,88,88,86,85,84,83,81,81,81,84,87,86,85,83,82,81,79,79,79,80,82,83,84,85,88,87,85,84,86,86,88,87
,88,89,91,93,92,90,91,92,92,92,92,90,89,91,91,90,89,89,90,88,88,89,90,94,98,100,99,97,95,93,90,88,87,89,91,93,92,95,97,100,98,97,94,94,96,94,94,91,90,91,91,93,95,96,95,96,96,99,100,99,97,95,96,95,98,
  100,102,102,103,105,104,106,108,109,108,108,105,105,105,102,103,103,103,102,101,104,104,106,107,109,110,112,116,116,114,114,112,110,107,105,103,101,98,93,94,96,99,98,100,101,99,97,98,98,99,99,101,100,
101,101,105,107,111,113,112,110,110,112,114,116,117,116,114,112,110,107,105,103,101,103,104,107,106,108,111,109,107,110,112,114,116,118,119,118,117,115,113,109,108,106,104,
  };

const int8_t spiral600Xdec[] PROGMEM = {    // 
  2,3,3,1,5,8,6,5,2,8,10,8,6,9,1,8,6,8,2,8,5,8,3,4,9,6,2,4,8,1,0,6,7,8,8,9,8,9,5,9,0,1,1,7,6,7,2,8,2,3,9,4,2,1,5,6,2,9,4,7,5,0,0,4,9,2,2,9,6,8,7,9,8,7,10,5,10,2,1,5,6,4,4,10,8,6,2,6,4,5,1,9,7,6,3,7,8,0,
3,7,10,3,0,9,10,5,6,2,0,6,3,0,9,4,3,9,6,0,8,10,10,3,2,6,8,6,10,10,9,6,2,6,0,7,9,8,5,9,0,4,9,8,10,3,8,5,9,1,8,7,1,7,9,9,2,2,9,4,2,7,7,2,10,9,1,9,1,2,4,0,10,8,7,1,9,1,2,2,1,5,3,7,6,3,8,2,6,3,0,7,5,8,6,3
,10,3,8,9,7,3,6,8,1,1,7,6,6,9,9,3,1,7,7,9,4,1,2,8,1,1,7,1,8,8,10,0,9,9,9,1,3,5,5,1,0,5,3,1,9,3,4,10,8,9,7,6,7,8,8,9,7,9,4,2,1,6,6,10,5,5,9,7,3,10,8,9,2,8,7,7,8,6,7,6,8,10,4,8,2,1,0,7,5,0,3,9,9,2,7,7,1
,8,6,1,6,5,2,4,2,9,8,3,6,3,6,9,10,7,6,8,6,2,3,5,5,0,10,1,5,1,2,8,5,5,7,0,4,4,3,1,9,4,3,2,5,5,5,0,7,3,4,4,2,1,1,3,10,2,9,1,4,6,6,2,6,3,1,10,7,0,6,10,2,3,9,10,2,7,8,6,5,2,3,5,4,8,7,6,5,2,2,5,6,8,9,4,1,3
  ,8,9,2,8,4,2,0,8,3,8,5,1,1,9,1,1,0,9,5,0,3,8,5,1,5,1,3,9,9,1,2,0,3,2,4,4,0,6,2,6,6,7,10,2,3,1,1,8,0,2,8,5,4,3,7,8,0,3,9,5,7,7,10,9,4,6,7,2,6,4,10,1,1,3,9,9,9,0,1,4,3,10,6,2,5,7,8,3,6,3,8,10,2,1,1,2,8,
6,6,0,3,2,7,7,1,7,6,9,0,6,7,9,3,9,6,6,4,7,3,6,6,6,6,7,5,2,1,10,8,4,8,4,7,2,6,6,3,7,3,6,6,7,8,1,3,4,1,1,0,9,8,6,1,6,8,5,7,1,7,8,7,8,8,3,0,10,1,5,2,3,4,3,4,6,6,4,3,3,2,2,0,8,4,8,9,9,6,1,5,10,5,6,2,3,7,7
,5,8,1,4,4,3,1,4,8,2,5,7,9,7,3,7,
  };

const int8_t spiral600Y[] PROGMEM = {    // 
  88,88,89,89,91,91,93,92,91,90,89,90,92,93,94,96,95,95,96,102,104,107,106,105,100,95,93,92,93,95,97,100,100,102,101,102,103,101,98,96,94,97,97,99,99,99,98,98,96,92,90,91,93,96,98,100,100,102,102,101,103
,104,105,107,106,108,110,109,108,104,104,102,97,95,95,93,95,96,97,99,104,109,112,114,111,110,109,104,99,97,95,93,92,90,91,93,93,95,97,103,104,104,105,106,113,115,113,117,115,115,113,107,106,106,107,108
,111,113,111,109,111,111,106,103,102,109,110,114,116,118,119,117,118,117,112,101,99,99,101,101,103,102,105,109,111,110,108,107,108,115,111,114,112,113,115,116,118,116,115,115,117,119,118,116,117,114
,115,116,113,111,110,109,106,104,102,100,100,99,102,104,108,110,113,114,113,111,109,102,99,97,95,95,97,98,100,101,104,107,106,106,108,107,106,105,103,101,97,95,93,93,95,98,98,99,100,101,106,108,105,105
  ,103,105,108,110,112,112,110,109,108,111,112,114,114,112,107,104,102,103,103,105,105,108,110,108,103,101,101,103,101,98,96,95,93,92,90,89,91,90,91,89,92,92,93,91,93,95,97,99,98,98,97,100,102,99,99,96
,94,92,95,96,96,94,97,99,99,97,95,95,93,90,91,92,90,89,90,90,89,90,93,94,93,91,89,88,88,88,83,82,80,85,88,88,86,84,82,80,82,84,85,87,87,88,91,92,91,89,87,86,82,82,82,79,79,75,74,75,77,76,78,79,80,81,79
,78,77,80,83,84,84,85,86,85,82,76,73,71,72,70,71,73,73,75,77,80,82,84,84,86,87,84,86,88,87,85,85,83,83,81,80,77,77,71,69,70,72,75,77,75,73,68,66,66,67,69,68,65,64,64,62,62,68,70,73,71,73,74,76,77,74,
73,69,67,69,70,68,66,65,65,67,72,76,78,75,75,72,66,64,64,62,61,63,64,70,77,79,79,81,81,83,87,85,84,84,82,82,80,79,77,72,69,67,69,71,75,78,79,76,73,72,68,66,65,60,61,63,63,62,63,62,61,60,61,62,63,64,66
  ,66,64,65,66,67,67,70,74,78,81,86,90,91,93,86,87,85,83,80,78,76,73,71,72,72,70,69,69,68,64,67,69,68,71,70,70,72,77,79,77,79,81,82,80,78,74,72,70,66,64,66,68,68,70,70,76,78,78,75,75,75,75,75,76,75,74,74
,76,76,78,80,81,84,83,83,85,87,88,90,89,85,79,77,73,72,72,73,77,77,79,82,82,83,84,89,88,87,86,87,87,88,86,85,83,83,85,85,84,83,81,81,79,80,81,82,85,88,87,85,85,88,89,88,90,
  };

const int8_t spiral600Ydec[] PROGMEM = {    // 
  3,5,1,4,10,3,3,4,2,8,1,2,1,0,6,7,0,2,7,9,8,9,5,8,9,2,7,4,7,5,10,2,2,4,10,3,8,7,0,3,7,2,8,8,0,8,6,5,4,4,8,5,5,8,4,3,4,4,1,7,6,1,6,6,9,9,10,9,6,4,8,5,8,7,8,8,2,8,6,9,8,1,7,8,10,7,3,5,1,0,0,8,1,7,2,2,1,5
,7,2,6,6,7,9,2,2,4,3,4,3,4,1,5,1,2,8,0,1,3,2,4,2,9,9,10,6,7,4,2,9,3,9,5,2,5,1,6,7,6,3,8,7,6,4,7,4,2,3,5,4,4,3,6,6,0,8,4,10,9,8,2,6,4,9,8,3,3,1,5,7,8,7,4,5,4,3,4,10,6,6,3,0,1,6,1,6,9,3,1,3,9,3,6,9,5,6,
4,10,3,6,3,8,9,9,9,8,7,7,7,3,3,0,1,7,8,3,1,3,9,6,2,1,1,6,9,7,2,7,9,5,6,3,8,3,6,9,8,4,7,9,5,1,5,3,6,3,6,8,6,7,8,8,7,9,8,6,4,2,7,3,3,2,8,8,4,3,3,4,10,4,5,7,5,10,1,5,1,9,2,1,8,7,5,5,3,3,5,6,9,8,7,8,3,5,0
,3,2,9,8,10,3,1,9,3,1,4,10,3,6,10,1,8,5,3,5,5,3,0,6,1,5,8,9,8,0,5,1,4,5,1,1,9,8,9,6,7,1,7,7,0,8,4,6,6,9,0,2,2,9,7,9,5,6,7,8,7,6,5,7,7,1,1,7,6,5,3,2,1,9,5,4,1,9,8,7,4,6,5,4,8,5,1,0,8,10,2,6,4,2,7,6,10,
  6,7,5,6,10,7,7,7,6,7,3,5,9,8,0,7,6,3,9,2,2,10,8,7,2,9,8,2,5,4,7,3,10,6,6,8,10,2,4,2,9,7,5,3,2,6,9,1,3,3,3,4,7,8,6,3,9,3,10,9,3,3,3,8,3,6,1,8,8,1,10,4,0,9,3,5,2,1,3,8,6,6,8,3,2,9,6,5,7,6,8,4,8,3,5,0,7,
9,3,4,4,3,8,5,6,1,2,6,5,8,1,4,3,8,4,3,7,3,8,6,8,3,5,6,7,7,2,3,2,1,1,1,7,2,3,4,2,5,7,2,8,1,6,2,1,2,5,2,1,5,6,6,4,6,8,3,1,3,0,3,5,3,5,6,9,2,2,4,10,9,2,6,3,6,1,8,7,7,4,2,1,3,4,0,5,10,4,6,5,3,7,3,7,5,7,9,
5,2,2,10,5,1,10,0,1,8,10,3,
  };


// The scanning path (servo positions) for 1000 point spiral scan
const int8_t spiral1000X[] PROGMEM = {    // 
  115,116,114,108,102,100,96,93,94,94,95,97,98,99,103,107,109,111,113,112,113,114,115,115,117,112,111,109,108,106,106,105,107,109,116,116,116,114,116,118,117,116,114,114,113,113,112,111,110,110,107,106,
105,107,108,110,113,113,113,113,111,109,110,113,114,115,112,110,108,106,105,104,103,102,100,99,97,95,97,97,96,98,100,102,102,103,103,101,97,95,95,99,101,102,104,102,100,101,102,104,106,106,108,108,103
,103,102,100,99,94,92,92,93,94,94,95,97,98,99,102,104,105,108,107,105,106,107,107,107,109,109,108,109,109,107,109,109,111,111,111,111,111,105,100,98,97,90,90,92,95,97,97,95,97,99,102,104,104,106,106,103
,101,103,104,105,102,101,100,99,98,96,99,97,94,93,95,97,99,100,99,100,102,99,98,98,101,103,104,102,101,100,100,98,97,96,94,93,93,94,94,93,92,90,93,95,96,96,94,95,95,95,91,89,90,90,86,85,88,91,92,92,
  91,92,94,96,97,96,92,93,95,96,97,98,99,98,94,93,91,89,88,87,88,87,89,90,91,91,92,93,93,92,90,90,87,86,86,87,89,90,90,89,91,91,90,89,87,88,92,89,85,85,83,82,82,84,87,89,88,85,83,85,85,87,88,86,83,84,86
,86,88,89,90,89,87,86,85,84,81,79,76,72,73,74,76,77,77,79,81,82,83,85,85,87,89,88,86,86,84,84,83,83,82,81,80,83,81,78,77,75,73,73,72,74,77,79,83,81,77,76,79,79,82,81,80,80,81,79,77,79,79,76,75,74,75,78
,79,77,76,76,74,74,75,82,83,82,82,80,79,77,76,74,72,73,71,69,71,72,70,67,66,68,68,69,70,72,71,69,66,67,68,71,71,65,72,75,79,80,79,77,77,78,77,76,74,74,76,72,66,67,63,60,61,61,62,65,67,68,70,72,74,79,
81,82,84,86,87,88,86,84,82,81,83,79,77,75,74,69,68,67,68,69,69,68,68,66,65,65,64,63,64,63,64,65,71,72,75,76,75,73,74,75,77,79,81,83,84,84,82,81,81,80,78,73,71,72,76,77,80,82,82,75,72,69,65,65,64,62,64
  ,61,60,60,61,61,60,62,63,62,64,63,62,61,62,63,64,66,65,65,65,66,67,73,74,71,70,68,69,70,71,71,69,68,66,67,69,71,71,69,67,66,67,68,70,70,71,74,72,66,66,64,63,61,63,70,74,75,80,84,87,88,90,89,87,86,86,85
,85,81,76,72,67,69,68,74,77,78,76,76,74,75,73,73,71,69,69,68,64,64,65,73,75,78,81,80,80,79,78,76,78,80,84,87,88,88,86,83,81,79,78,77,76,71,71,67,65,62,62,64,65,67,67,65,64,64,65,67,68,72,69,69,69,70,
72,72,72,71,71,71,74,78,79,79,78,78,76,74,74,74,73,73,72,70,69,67,69,70,73,75,76,77,75,74,73,75,76,78,80,78,76,76,77,76,76,78,77,81,82,84,86,83,84,84,86,87,89,90,90,86,81,79,78,80,81,82,82,83,84,83,82
,81,80,79,81,83,84,87,86,85,83,83,81,83,81,80,79,84,86,87,85,84,86,90,89,93,95,96,94,91,89,87,87,85,83,83,84,85,88,89,88,87,86,85,85,83,83,82,79,80,79,81,81,83,85,88,89,91,91,93,92,90,88,89,88,90,90,92
  ,92,93,92,91,89,88,87,88,86,87,86,87,88,92,90,89,91,92,94,95,98,97,95,94,95,101,94,96,98,97,93,92,93,94,95,99,102,103,96,96,101,100,95,95,94,94,92,92,93,98,102,104,103,101,99,97,95,95,96,101,99,98,98
,96,96,90,91,88,89,90,90,93,93,92,94,96,99,100,98,98,96,96,99,97,96,91,92,91,91,92,94,97,99,101,102,104,106,105,107,106,107,105,103,106,106,105,103,104,103,105,100,102,99,98,99,99,98,99,101,103,104,104
,106,108,108,109,109,110,111,110,111,109,107,109,112,112,108,105,104,100,98,98,101,102,101,104,106,110,110,108,104,102,102,100,97,95,101,102,105,107,107,108,111,113,114,113,115,116,115,113,112,112,114
,115,116,114,113,114,115,117,118,117,116,109,105,103,103,105,108,108,110,111,111,113,113,111,110,107,107,105,104,108,109,111,110,108,107,107,106,103,104,105,106,108,111,112,113,112,106,101,100,101,92
,99,102,109,110,115,115,114,116,115,118,119,117,118,119,117,116,116,117,118,118,119,118,
  };

const int8_t spiral1000Xdec[] PROGMEM = {    // 
  3,0,4,7,6,5,2,3,4,3,5,7,9,9,3,5,7,6,4,7,6,4,1,7,3,2,5,6,7,4,4,2,6,8,1,8,1,10,6,0,7,4,8,4,10,4,6,8,8,7,4,8,9,4,9,8,0,3,4,3,2,2,8,1,10,0,7,3,2,1,3,6,7,2,6,3,4,8,6,5,9,1,6,0,4,9,8,4,0,9,5,0,6,2,3,6,10,1,
8,5,4,2,2,1,8,1,5,9,3,5,5,9,9,1,5,9,4,10,3,7,3,9,7,10,7,5,1,6,9,6,8,1,9,9,8,4,7,5,7,6,5,2,1,9,9,9,1,10,3,8,7,7,9,6,3,9,6,6,4,2,9,9,4,9,5,4,1,6,10,4,8,4,9,1,0,2,1,2,5,7,10,4,9,8,6,9,4,2,7,3,4,8,10,2,6,
8,1,8,6,7,1,4,9,8,3,1,1,3,4,9,1,8,9,5,9,6,1,10,4,2,1,4,8,0,2,4,3,4,8,1,3,5,4,9,5,0,1,3,7,9,6,3,9,9,8,4,9,1,3,2,8,5,2,1,4,2,4,2,2,8,9,0,9,3,2,9,4,2,5,7,3,7,9,1,2,4,5,4,0,9,1,5,5,6,3,6,10,8,4,9,8,9,4,10
,7,1,7,6,10,3,7,7,8,1,6,7,9,6,0,7,3,10,6,1,3,9,2,7,3,9,7,3,4,6,1,2,4,5,2,6,1,6,2,7,5,1,4,9,8,1,10,5,0,8,9,2,5,9,3,2,9,9,3,3,4,5,7,0,1,3,3,10,8,2,5,3,8,3,10,4,5,8,2,6,5,2,6,9,9,7,3,9,9,5,1,2,6,5,4,9,5,
  1,10,7,9,8,5,1,8,0,3,9,1,6,3,4,10,8,0,0,7,8,9,2,8,3,0,5,0,7,6,2,9,7,3,7,1,2,5,9,10,6,4,7,10,3,10,5,0,2,7,6,3,2,8,5,6,3,2,5,6,3,0,2,7,2,8,0,4,2,9,10,3,10,3,10,5,8,4,4,1,9,5,9,6,1,3,3,1,9,6,0,9,8,5,2,10
,3,9,9,5,9,2,6,2,5,8,3,7,7,3,2,2,7,3,2,9,1,1,9,0,2,8,7,8,7,8,9,1,8,7,8,7,2,4,2,7,4,9,10,7,0,5,2,4,4,7,2,4,8,7,3,4,2,4,3,2,2,1,0,3,4,3,1,5,7,6,8,10,2,1,5,6,7,6,10,5,9,4,9,3,8,5,9,0,5,9,4,2,6,6,8,8,2,9,
3,9,7,9,2,3,3,6,8,5,5,9,3,1,1,4,1,7,10,9,7,3,8,5,6,8,2,2,9,8,2,4,8,5,3,10,10,8,7,1,4,9,7,2,8,7,0,2,3,6,5,5,5,1,8,10,3,5,2,3,1,5,2,6,5,7,10,1,6,6,1,0,2,4,1,9,4,8,8,1,4,7,1,1,6,8,0,3,4,5,3,5,9,1,4,2,3,5
,5,7,8,7,7,5,5,8,8,7,9,5,2,5,10,4,8,1,6,5,2,10,1,1,9,8,9,3,9,2,3,7,3,1,2,9,0,0,9,1,3,4,7,7,5,5,9,1,1,3,4,4,2,2,9,5,4,3,9,0,3,4,8,1,1,8,5,3,9,1,6,0,7,6,8,3,8,8,4,4,9,8,1,8,6,5,8,6,3,9,3,6,4,0,6,2,5,4,8
  ,3,2,8,8,0,3,2,2,1,9,1,7,9,3,5,2,1,4,7,9,9,6,2,9,2,1,2,5,4,8,2,9,0,7,7,3,7,0,10,3,8,5,0,10,7,5,9,8,7,9,2,10,1,1,10,1,6,4,8,3,9,2,1,1,5,5,4,10,6,2,8,4,8,6,5,9,7,1,2,6,5,5,1,5,1,8,5,10,9,4,8,5,2,8,9,6,3
,1,0,2,2,3,8,1,5,8,8,7,2,7,7,0,6,8,6,9,6,5,8,4,6,0,4,1,2,4,4,0,3,9,3,7,3,1,3,6,1,6,8,2,1,3,8,3,8,2,6,9,5,1,5,8,0,2,10,6,8,5,8,3,1,7,9,4,8,7,1,2,8,2,8,0,4,5,3,8,5,4,9,4,2,8,7,5,0,1,4,1,3,10,8,2,7,5,3,2
,3,6,2,5,1,5,4,9,8,2,2,10,9,8,5,6,9,7,3,9,2,4,
  };

const int8_t spiral1000Y[] PROGMEM = {    // 
  92,90,91,92,89,88,88,89,87,90,89,88,89,91,90,91,91,90,89,91,93,94,95,97,97,98,96,96,95,95,92,93,94,93,93,95,99,102,102,100,98,100,101,99,97,96,94,93,92,95,96,99,101,100,99,99,101,102,104,106,109,110,110
,107,106,104,99,98,97,97,99,98,96,97,97,96,97,95,96,94,92,91,92,91,93,94,92,90,90,91,93,97,99,101,102,102,103,104,104,103,104,103,105,103,100,99,95,96,94,92,90,92,94,95,100,100,101,101,103,107,108,109
,112,113,114,113,111,110,108,108,107,106,105,104,102,100,102,102,104,105,107,101,96,94,92,86,88,91,94,97,103,104,106,106,106,106,105,107,106,107,110,110,111,112,110,109,106,101,99,99,99,104,111,113,
114,115,112,112,113,115,116,116,117,118,116,114,115,114,113,112,109,107,108,108,109,109,108,107,104,102,98,95,95,97,98,102,104,105,107,111,112,115,119,116,114,111,110,108,108,110,113,118,117,116,113,115
  ,116,119,118,117,118,117,113,111,109,110,112,111,115,113,114,111,110,107,106,104,102,100,101,103,105,109,112,112,109,106,102,102,103,101,99,98,97,96,98,99,100,106,110,113,111,108,105,103,104,105,105
,103,107,106,105,102,107,106,104,102,101,101,99,95,94,93,92,92,95,96,98,102,103,102,105,107,109,111,112,114,117,117,118,118,117,119,118,117,116,117,115,116,114,112,110,109,106,104,101,100,103,105,104,
104,102,98,98,98,98,97,97,102,104,105,106,107,108,110,113,115,115,116,114,112,107,105,105,107,107,108,111,114,116,114,112,112,115,116,113,111,112,110,109,109,111,111,109,109,110,110,112,112,109,107,108
,106,108,107,107,105,105,104,102,103,102,98,101,100,98,96,95,94,92,94,95,97,97,95,97,95,95,96,94,95,94,96,98,101,105,106,104,103,103,102,101,99,99,99,97,97,94,94,94,94,95,96,99,100,100,100,101,101,100
  ,99,99,97,95,97,98,97,99,98,99,100,102,104,103,100,96,94,91,90,90,92,92,90,89,89,90,91,92,92,91,93,92,91,93,93,91,89,88,87,87,88,87,89,89,93,95,96,97,94,93,92,88,87,86,84,83,87,85,88,90,89,90,91,92,91
,91,89,87,86,84,84,87,86,86,85,88,86,88,84,82,84,85,85,87,91,94,96,95,92,89,90,93,93,92,90,88,81,81,79,84,83,82,81,81,84,85,90,89,88,89,90,90,91,92,90,88,86,85,84,85,82,82,81,81,80,78,79,77,78,79,80,
78,80,78,79,79,79,81,83,82,83,82,82,80,82,83,86,86,87,88,87,86,85,83,84,85,83,85,84,82,81,78,76,78,78,80,78,78,76,74,73,74,76,74,73,71,69,67,70,71,73,75,77,75,74,72,70,68,71,72,73,75,75,77,75,76,74,73
,69,71,72,73,76,76,74,68,65,64,67,68,68,66,67,70,71,70,71,73,74,72,69,65,63,62,64,68,70,75,76,77,77,79,78,79,82,80,77,74,71,69,66,63,62,63,61,62,63,64,66,64,66,67,67,71,73,77,80,81,81,78,78,75,73,70,65
  ,61,62,63,64,66,66,70,74,75,75,74,73,75,76,75,72,69,67,68,70,72,78,79,80,81,83,83,85,84,82,80,80,78,76,76,75,73,67,65,64,64,71,72,73,72,73,72,66,65,63,62,64,61,60,61,61,60,61,62,69,68,70,71,69,68,67,
68,69,70,71,72,71,69,70,69,68,64,64,64,65,63,67,66,65,63,61,61,62,64,66,71,75,79,79,82,84,86,87,85,83,76,75,71,67,63,63,62,62,66,67,71,76,81,83,82,84,85,87,87,86,84,82,81,79,78,77,78,83,77,74,73,76,79
,85,84,86,88,81,80,78,76,76,75,72,70,69,69,69,71,71,73,75,78,78,76,74,73,73,74,76,76,73,73,70,69,68,66,67,65,66,66,64,66,66,66,68,71,75,79,77,75,70,70,69,68,71,72,78,80,81,80,80,78,77,71,68,67,67,72,73
,73,80,78,80,82,81,81,83,82,81,80,78,76,74,74,75,77,77,76,74,73,76,78,79,80,81,81,84,83,82,83,82,80,78,77,83,83,84,85,84,82,83,81,79,80,82,83,85,87,89,88,86,86,86,85,81,80,82,83,85,88,89,91,90,90,87,
88,87,86,87,87,86,85,84,87,86,88,89,89,87,85,86,84,84,86,87,88,90,89,88,91,93,94,96,92,91,
  };

const int8_t spiral1000Ydec[] PROGMEM = {    // 
  6,5,2,7,2,8,1,3,10,6,5,7,7,0,10,6,2,6,10,8,1,5,10,5,1,0,6,7,3,2,9,10,0,9,10,5,2,7,5,5,8,8,0,4,9,4,9,6,3,2,5,0,5,5,4,6,2,8,4,1,3,6,10,8,1,4,6,1,9,6,10,5,10,6,9,2,7,5,0,4,9,5,5,9,7,9,9,2,4,6,9,9,5,2,3,8
,1,8,4,8,7,1,3,7,7,1,6,0,5,4,9,4,3,10,0,8,3,5,2,6,6,4,2,8,8,2,7,1,5,10,3,9,7,1,1,9,5,7,3,10,6,1,2,2,9,8,7,3,0,0,1,9,3,5,6,0,4,0,2,8,2,7,7,5,10,2,4,4,7,6,3,9,1,7,9,1,5,5,6,0,0,9,4,7,3,6,5,1,2,2,5,10,2,
1,6,4,9,4,1,2,10,9,3,5,6,8,6,8,9,1,4,10,3,9,4,1,0,8,3,3,3,4,4,3,9,2,4,7,7,6,8,6,9,0,7,8,1,6,4,8,7,10,0,5,0,3,4,2,3,4,2,7,7,10,0,8,6,1,3,3,9,8,5,9,1,0,7,8,9,8,10,8,3,8,2,5,1,9,9,6,5,8,9,6,2,7,3,2,5,9,1
,0,2,9,7,2,0,3,1,3,9,7,5,2,8,5,6,2,8,4,8,3,6,8,3,1,5,2,5,8,9,6,1,6,2,7,7,7,2,2,3,7,9,8,3,10,5,0,0,2,8,5,2,2,9,6,10,2,3,5,6,9,10,7,3,8,0,6,3,6,9,9,1,8,4,6,1,7,2,4,2,1,4,2,5,9,8,5,10,8,4,8,9,3,6,6,2,0,6
  ,2,1,3,8,5,10,8,3,3,9,3,7,2,3,6,3,8,3,2,1,1,10,3,8,2,5,2,7,7,10,4,5,1,6,7,7,0,5,1,5,4,10,3,9,4,6,7,7,1,4,1,7,7,0,0,3,3,6,6,9,9,3,7,2,2,10,1,4,8,0,1,7,3,3,9,8,3,8,5,8,9,9,7,6,0,5,6,6,2,0,4,6,1,6,1,2,3,
5,1,9,5,6,8,2,6,10,1,5,5,6,2,1,5,8,9,0,8,8,4,5,2,7,5,6,4,5,9,0,1,7,1,6,10,3,3,7,5,8,1,1,3,1,10,2,6,7,5,3,7,10,5,3,9,2,3,5,7,5,4,7,0,6,1,5,2,6,7,5,7,3,8,6,2,9,7,7,1,5,1,7,5,10,0,2,7,6,9,5,1,9,4,3,2,3,1
,9,0,7,8,7,2,7,3,5,9,8,8,6,2,7,2,5,9,1,1,2,5,2,4,8,1,7,5,8,0,4,7,2,0,7,3,3,4,8,2,1,5,8,7,2,5,1,4,0,8,5,9,3,4,0,6,7,6,4,10,4,7,8,1,0,6,3,9,2,1,6,9,8,2,7,5,6,4,8,2,9,3,6,2,0,5,5,2,2,1,10,3,5,8,1,6,8,5,6
,3,4,6,8,1,9,2,6,5,10,9,1,5,9,0,9,6,2,4,4,1,2,5,9,10,2,5,7,4,2,1,0,8,5,1,2,8,1,5,9,5,3,1,4,4,7,2,4,0,2,6,5,4,5,8,8,1,6,7,3,3,9,6,7,10,9,6,1,10,9,3,5,1,8,3,0,9,5,5,8,4,1,8,4,10,5,6,7,5,5,5,7,1,8,6,4,6,
  8,10,3,5,8,5,6,7,3,1,8,10,6,0,8,5,3,4,9,8,4,2,6,2,9,2,4,10,1,4,2,7,7,8,10,1,7,2,9,6,6,3,8,4,3,9,3,6,1,9,0,10,7,6,2,5,6,4,8,4,9,9,1,6,7,5,5,5,0,8,6,7,4,5,2,1,4,5,7,9,1,3,10,8,7,10,5,5,1,6,8,2,0,5,3,6,2
,3,5,2,7,4,3,9,8,5,1,8,5,1,7,9,2,8,9,1,5,1,6,2,9,10,1,7,4,2,8,4,5,2,1,6,0,9,4,6,10,2,9,6,1,9,5,1,3,4,5,3,3,3,6,9,7,6,7,5,0,9,3,6,4,10,2,4,6,2,10,9,7,8,10,1,8,3,6,6,1,9,9,3,8,2,9,6,0,8,9,6,2,8,8,10,9,1
,7,4,0,1,7,4,6,5,2,0,9,0,8,9,3,3,7,2,6,3,9,4,9,6,6,1,
  };

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

float Xangle = 90.0;
float Yangle = 90.0;

int XAngle2Microseconds;
int YAngle2Microseconds;

void setup(){
  XServo.attach(Xpin);
  YServo.attach(Ypin);
  XServo.write(Xangle+Xoffset);
  YServo.write(Yangle+Yoffset);

  pinMode(TriggerPin, OUTPUT); // Set pin as OUTPUT
  Serial.begin(9600); // Start serial communication at 9600 bps
}

// Communication via Serial Monitor
void loop(){
  digitalWrite(TriggerPin, LOW); // trigger low
  if(Serial.available()) {
    sCmd = Serial.read();
    switch (sCmd) {

    case 'd':  // Calibrate to the right
      Xangle = 90.0;
      Xoffset+=1.0;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      delay(move_ms);  
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      Serial.print("X offset: ");
      Serial.println(Xoffset);
      break;
      
    case 'a':  // Calibrate to the left
      Xangle = 90.0;
      Xoffset-=1.0;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);  
      delay(move_ms);         
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);
      Serial.print("X offset: ");
      Serial.println(Xoffset);
      break;

    case 's':  // Calibrate downwards
      Yangle = 90.0;    
      Yoffset+=1.0;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds); 
      delay(move_ms);
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);
      Serial.print("Y offset: ");
      Serial.println(Yoffset);
      break;
      
    case 'w':  // Calibrate upwards
      Yangle = 90.0;    
      Yoffset-=1.0;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;      
      YServo.writeMicroseconds(YAngle2Microseconds);
      delay(move_ms);     
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);
      Serial.print("Y offset: ");
      Serial.println(Yoffset);
      break;      

    case 'z':  // Set both offsets to the Xoffset and Yoffsets determined in the beginning
      Xangle = 90.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 90.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      delay(move_ms);
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      Serial.print("X offset: ");
      Serial.println(Xoffset);
      Serial.print("Y offset: ");
      Serial.println(Yoffset);
      break;
     
    case 'c':  // Jump to 30 degree extremes clockwise
      Serial.println("Checking 30 degree extreme points clockwise:");
      Xangle = 90.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 90.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("center...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);
      
      Xangle = 90.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 120.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("up...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);
      
      Xangle = 120.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 90.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("right...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);
      
      Xangle = 90.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 60.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("down...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);    
  
      Xangle = 60.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 90.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("left...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);
      
      Xangle = 90.0+Xoffset;
      XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
      XServo.writeMicroseconds(XAngle2Microseconds);
      Yangle = 90.0+Yoffset;
      YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
      YServo.writeMicroseconds(YAngle2Microseconds);      
      Serial.print("center...");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      delay(1000);
      Serial.println("done!");

      break;

    case 't':  // Send a single trigger
      Serial.println("Trigger sent");
      digitalWrite(TriggerPin, HIGH);       // Trigger
      delay(trigger_ms);
      digitalWrite(TriggerPin, LOW);  
      break;      

    case 'r':  // Send 50 triggers without moving the servos
      Serial.println("Sending 50 Triggers...");
      int iCond;
      for(iCond=0; iCond<50; iCond++) {   
        digitalWrite(TriggerPin, HIGH);       // Trigger
        delay(trigger_ms);
        digitalWrite(TriggerPin, LOW);  
        delay(sampling_time_ms-trigger_ms);
      }
      Serial.println("done...");
      break;

    case 'q':  // Switch between different scan modes
      scanmode+=1;
      if (scanmode>=nScanmodes) {scanmode = 0;}
      Serial.print("Scan mode: ");  
      Serial.println(scanmode);        
      if (scanmode==0){Serial.println("100 point square @30 deg");}
      if (scanmode==1){Serial.println("300 point spiral @30 deg");}
      if (scanmode==2){Serial.println("600 point spiral @30 deg");}
      if (scanmode==3){Serial.println("1000 point spiral @30 deg");}
      if (scanmode==4){Serial.println("20 Hz Eyemove");}     
      break;
          
    case 'p':  // Run the selected scan
      // Array Stimulus
      Serial.println("Playing Arrays...");  
      PlayArrays(); 
      break;

    }
  }

  delay(100); 
}

//////////////////////////////////////////
/// Playing the chosen scan path array ///
//////////////////////////////////////////

void PlayArrays () 
{
  int  nConds;
  int  iCond;

  // Enter the array size here
  if (scanmode==0) {nConds=100;}
  if (scanmode==1) {nConds=300;} 
  if (scanmode==2) {nConds=600;}  
  if (scanmode==3) {nConds=1000;} 
  if (scanmode==4) {nConds=1200;} 

  for(iCond=0; iCond<nConds; iCond++) {

    if (scanmode==0) { // 100 point 300 deg rectangle
      Xangle=pgm_read_byte_near(square100X + iCond);
      float Xangle_addon = pgm_read_byte_near(square100Xdec + iCond);
      Xangle_addon/=10;
      Xangle+=Xangle_addon;
      Yangle=pgm_read_byte_near(square100Y + iCond);
      float Yangle_addon = pgm_read_byte_near(square100Ydec + iCond);
      Yangle_addon/=10;
      Yangle+=Yangle_addon;
    }
    else if (scanmode==1) { // 300 point 30 deg spiral
      Xangle=pgm_read_byte_near(spiral300X + iCond);
      float Xangle_addon = pgm_read_byte_near(spiral300Xdec + iCond);
      Xangle_addon/=10;
      Xangle+=Xangle_addon;
      Yangle=pgm_read_byte_near(spiral300Y + iCond);
      float Yangle_addon = pgm_read_byte_near(spiral300Ydec + iCond);
      Yangle_addon/=10;
      Yangle+=Yangle_addon;
    }
    else if (scanmode==2) { // 600 point 30 deg spiral
      Xangle=pgm_read_byte_near(spiral600X + iCond);
      float Xangle_addon = pgm_read_byte_near(spiral600Xdec + iCond);
      Xangle_addon/=10;
      Xangle+=Xangle_addon;
      Yangle=pgm_read_byte_near(spiral600Y + iCond);
      float Yangle_addon = pgm_read_byte_near(spiral600Ydec + iCond);
      Yangle_addon/=10;
      Yangle+=Yangle_addon;
    }
    else if (scanmode==3) { // 1000 point 30 deg spiral
      Xangle=pgm_read_byte_near(spiral1000X + iCond);
      float Xangle_addon = pgm_read_byte_near(spiral1000Xdec + iCond);
      Xangle_addon/=10;
      Xangle+=Xangle_addon;
      Yangle=pgm_read_byte_near(spiral1000Y + iCond);
      float Yangle_addon = pgm_read_byte_near(spiral1000Ydec + iCond);
      Yangle_addon/=10;
      Yangle+=Yangle_addon;
    }
    else if (scanmode==4) { // Eyemove at 20 Hz
      Xangle=pgm_read_byte_near(EyemoveX + iCond);
      float Xangle_addon = pgm_read_byte_near(EyemoveXdec + iCond);
      Xangle_addon/=10;
      Xangle+=Xangle_addon;
      Yangle=pgm_read_byte_near(EyemoveY + iCond);
      float Yangle_addon = pgm_read_byte_near(EyemoveYdec + iCond);
      Yangle_addon/=10;
      Yangle+=Yangle_addon;
    }

    XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
    XServo.writeMicroseconds(XAngle2Microseconds);
    YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
    YServo.writeMicroseconds(YAngle2Microseconds);

    delay(move_ms);

    digitalWrite(TriggerPin, HIGH);
    delay(trigger_ms);
    digitalWrite(TriggerPin, LOW);

    Serial.print(iCond+1);
    Serial.print("/");
    Serial.println(nConds);

    delay(sampling_time_ms-trigger_ms);
  }

  // Reset to center
  digitalWrite(TriggerPin, HIGH);
  delay(trigger_ms);
  digitalWrite(TriggerPin, LOW);
  
  Xangle=90.0;
  XAngle2Microseconds = 1000 + (Xangle+Xoffset)/90 * 500;
  XServo.writeMicroseconds(XAngle2Microseconds);
  Yangle=90.0;
  YAngle2Microseconds = 1000 + (Yangle+Yoffset)/90 * 500;
  YServo.writeMicroseconds(YAngle2Microseconds);

}