/SloMomBox/SloMomBox.ino

#include "SPI.h"
#include <EEPROM.h>
#include "Wire.h"
#include <LiquidCrystal.h>
#include <MenuBackend.h>

LiquidCrystal lcd(0);

String inputString = "";         // a string to hold incoming data
String inputRobot = "";
boolean stringComplete = false;  // whether the string is complete
boolean stringRobotComplete = false;
int l;

byte address = 0x11;
int CSS= 49;
int i=0;

unsigned analogSensorsTotal = 16;
unsigned analogIn[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11, A12, A13, A14, A15};
unsigned analogVal[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
unsigned digitalIn[] = {22, 24, 26, 28, 30, 32, 34, 36};
unsigned digitalOut[] = {23, 25, 27, 29, 31, 33, 35, 37};
unsigned digitalVal[] = {0, 0, 0, 0, 0, 0, 0, 0};
unsigned keypadIn[] = {2, 3, 4, 5};

int sensorPin1 = A0; 
int sensorPin2 = A1; 
int sensorPin3 = A2; 
int sensorPin4 = A3; 
int sensorPin5 = A4; 
int sensorPin6 = A5; 
int sensorPin7 = A6; 
int sensorPin8 = A7;

int sensor1 = 0; 
int sensor2 = 0; 
int sensor3 = 0; 
int sensor4 = 0; 
int sensor5 = 0; 
int sensor6 = 0; 
int sensor7 = 0; 
int sensor8 = 0;

#define ESC_PIN    2
#define KB_PIN     3
#define DOWN_PIN   4
#define UP_PIN     5
#define pinEstop   7
#define pinBuzzer 12
#define txSerial  18
#define rxSerial  19
#define sdaLcd    20
#define sclLcd    21

#define soundActive tone(pinBuzzer, 2000, 200)
#define noSound noTone(pinBuzzer)

int lastButtonPushed = 0;

int lastButtonEnterState = LOW;  
int lastButtonEscState = LOW;   
int lastButtonLeftState = LOW;   
int lastButtonRightState = LOW;   

long lastEnterDebounceTime = 0;  
long lastEscDebounceTime = 0;  
long lastLeftDebounceTime = 0;  
long lastRightDebounceTime = 0;  
long debounceDelay = 100;

bool menurefreshed = true;

MenuBackend menu = MenuBackend(menuUsed,menuChanged);

      MenuItem miAnalog = MenuItem("Analog sens");
            MenuItem miASens1 = MenuItem(">");
            MenuItem miASens2 = MenuItem("<");
           
      MenuItem miDigital = MenuItem("Digital sens");
            MenuItem miDSens1 = MenuItem(">");
            MenuItem miDSens2 = MenuItem("<");

      MenuItem miSound = MenuItem("Sound");
            MenuItem miOn = MenuItem("on");
            MenuItem miOff = MenuItem("off");

int totalF1 = 0;
int readingsF1[10];
int indexF1 = 0;
int averageF1 = 0;
int valoriF1[100];
int indexvalF1 = 0;
int val_ant1;

void setup() {
  initialise_pins();
  initialise_features();
  //initialiseAnalogTune();
  for (int thisReadingF1 = 0; thisReadingF1 < 10; thisReadingF1++)
    readingsF1[thisReadingF1] = 0; 
  Serial.begin(115200);
  Serial1.begin(115200);
  pinMode (CSS, OUTPUT);
  SPI.begin();
  // reserve 200 bytes for the inputString:
  inputString.reserve(30);
  Serial.println("OK");
  Serial1.println("ROBOT INTF OK");
  
  lcd_default();
}

#define CMD_LEN 4
#define FRAME_START_ADDR 1000
#define BLENDER_FPS_ADDR 0
#define BNUM_FRAMES_ADDR 10

int value = 0;
int numFrames = 0;
int frame = 0;

char cmd[CMD_LEN+1] = {0};
char cmdRobot[CMD_LEN+1] = {0};

char charBuf[50];
char charBufRobot[50];

void loop() {
  if(stringRobotComplete) {
    Serial.println(inputRobot);
    memset(cmdRobot, 0, sizeof(cmd));
    inputRobot.toCharArray(charBufRobot, 50) ;
    strncpy(cmdRobot, charBufRobot, CMD_LEN);
    if(strcmp(cmdRobot, "RRUN") == 0) {
      Serial.println("Robot: RRUN");
      runProgram();
    } else if (strcmp(cmdRobot, "INIT") == 0) {
      digitalPotWrite(CSS, EEPROM.read(FRAME_START_ADDR));
      Serial.println("Robot: INIT");
    }
    inputRobot = "";
    stringRobotComplete = false;
  }
  
  if(stringComplete) {
    memset(cmd, 0, sizeof(cmd));
    //Serial.print("STRING: ");
    //Serial.println(inputString); 
    inputString.toCharArray(charBuf, 50) ;
      
    if(charBuf[0] == 'F') {
      strncpy(cmd, charBuf+2, CMD_LEN);
      //Serial.print("CMD: ");
      //Serial.println(cmd);
      if(strcmp(cmd, "MOVE") == 0) {
        // MOVE command
        value = atoi(charBuf+4);
        if(value < 0)
          value = 0;
        if(value > 255)
          value = 255;
        digitalPotWrite(CSS, value);
      } else if(strcmp(cmd, "STOR") == 0) {
        // STORe focus point
        // STOR addr focusVal
        frame = atoi(charBuf+4);
        //Serial.print("FRAME: ");
        //Serial.println(frame, DEC);
        
        value = atoi(charBuf+9);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        //Serial.print("VALUE: ");
        //Serial.println(value, DEC);
          
        EEPROM.write(FRAME_START_ADDR + frame, value);
        
        Serial.println("OK");
      } else if(strcmp(cmd, "BFPS") == 0) {
        // BFPS - blender frames per second
        // BFPS fpsVal numFrames
        value = atoi(charBuf+4);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        EEPROM.write(BLENDER_FPS_ADDR, value);
        
        value = atoi(charBuf + 9);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        EEPROM.write(BNUM_FRAMES_ADDR, value);
        
        Serial.print("FRAMES: ");
        Serial.println(EEPROM.read(BNUM_FRAMES_ADDR), DEC);
        
        Serial.print("FPS: ");
        Serial.println(EEPROM.read(BLENDER_FPS_ADDR), DEC);
      } else if(strcmp(cmd, "RRUN") == 0) {
        // RUN program
        runProgram();
      } else if(strcmp(cmd, "STOP") == 0) {
        // STOP program
        // This cannot be done here
      } else if(strcmp(cmd, "SHOW") == 0) {
        displayProgram();
      } else if(strcmp(cmd, "CLER") == 0) {
        Serial.print("ERASING..");
        for(int i=0; i<2000; i++) {
          if(i%500 == 0)
            Serial.print(".");
          EEPROM.write(i, 0);
        }
        Serial.println("DONE");
      } else{
        // Unknown command
        Serial.println("ERR: Unknown command");
      }
    } else if(charBuf[0] == 'D') {
      int output;
      
    } else if(charBuf[0] == 'A') {
      int output = -1;
      output = atoi(charBuf+1);
      Serial.println(output, DEC);
      char *tmpBuf = charBuf+3;
      
      strncpy(cmd, tmpBuf, CMD_LEN);
      Serial.println(cmd);
      if(strcmp(cmd, "MOVE") == 0) {
        // MOVE command
        value = atoi(tmpBuf+4);
        if(value < 0)
          value = 0;
        if(value > 255)
          value = 255;
        digitalPotWrite(CSS, value);
      } else if(strcmp(cmd, "STOR") == 0) {
        // STORe focus point
        // STOR addr focusVal
        frame = atoi(tmpBuf+4);
        //Serial.print("FRAME: ");
        //Serial.println(frame, DEC);
        
        value = atoi(tmpBuf+9);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        //Serial.print("VALUE: ");
        //Serial.println(value, DEC);
          
        EEPROM.write(FRAME_START_ADDR + frame, value);
        
        Serial.println("OK");
      } else if(strcmp(cmd, "BFPS") == 0) {
        // BFPS - blender frames per second
        // BFPS fpsVal numFrames
        value = atoi(tmpBuf+4);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        EEPROM.write(BLENDER_FPS_ADDR, value);
        
        value = atoi(tmpBuf + 9);
        if(value > 255)
          value = 255;
        if(value < 0)
          value = 0;
        EEPROM.write(BNUM_FRAMES_ADDR, value);
        
        Serial.print("FRAMES: ");
        Serial.println(EEPROM.read(BNUM_FRAMES_ADDR), DEC);
        
        Serial.print("FPS: ");
        Serial.println(EEPROM.read(BLENDER_FPS_ADDR), DEC);
      } else if(strcmp(cmd, "RRUN") == 0) {
        // RUN program
        runProgram();
      } else if(strcmp(cmd, "STOP") == 0) {
        // STOP program
        // This cannot be done here
      } else if(strcmp(cmd, "SHOW") == 0) {
        displayProgram();
      } else if(strcmp(cmd, "CLER") == 0) {
        Serial.print("ERASING..");
        for(int i=0; i<2000; i++) {
          if(i%500 == 0)
            Serial.print(".");
          EEPROM.write(i, 0);
        }
        Serial.println("DONE");
      } else{
        // Unknown command
        Serial.println("ERR: Unknown command");
      }
    }
    
    inputString = "";
    stringComplete = false;
  }
  if(emergencyStop() == '0'){
    //readPins();
    display_refresh();
    //serial_write();
    readButtons(); 
    navigateMenus();    
    noTone(pinBuzzer);
    //processAnalogInputs();
    if (!menurefreshed){
      lcd_default();
    }
  }else{
     alarm();
  }
}

// Run the stored points in EEPROM according to the fps 
void runProgram() {
  int frameDelay=1000/EEPROM.read(BLENDER_FPS_ADDR);
  int i=0;
  int numFrames = EEPROM.read(BNUM_FRAMES_ADDR);
  Serial.print("DELAY: ");
  Serial.println(frameDelay, DEC);
  
  for(i = 0; i<numFrames; i++) {
    value = EEPROM.read(FRAME_START_ADDR+i);
    digitalPotWrite(CSS, value);
    delay(frameDelay);
    Serial.print("GOTO: ");
    Serial.println(value);
    
    // TODO Check ESTOP signal
    // TODO Check serial STOP 
  } 
}

void displayProgram() {
  int numFrames = EEPROM.read(BNUM_FRAMES_ADDR);
  int fps = EEPROM.read(BLENDER_FPS_ADDR);
  int frameDelay=1000/fps; // 1000 ms/fps
  
  Serial.print("FRAMES: ");
  Serial.println(numFrames);
  Serial.print("FPS: ");
  Serial.println(fps, DEC);
  Serial.print("DELAY: ");
  Serial.println(frameDelay, DEC);
  
  Serial.println("BEGIN");
  
  for(int i=0; i<numFrames; i++) {
    int val = EEPROM.read(FRAME_START_ADDR + i);
    Serial.print("FOCUS POINT: ");
    Serial.println(val, DEC);
  }
  
  Serial.println("END");
}

// This receives data on interrupt from the serial port
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read(); 
    Serial1.print(inChar);
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if ((inChar == '\n') || (inChar == '\r')) {
      stringComplete = true;
    } 
  }
}

void serialEvent1() {
  while (Serial1.available()) {
    // get the new byte:
    char inChar = (char)Serial1.read(); 
    // add it to the inputString:
    inputRobot += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if ((inChar == '\n') || (inChar == '\r')) {
      stringRobotComplete = true;
    } 
  }  
}

int digitalPotWrite(int l, int value) {
  digitalWrite(l, LOW);
  SPI.transfer(address);
  SPI.transfer(value);
  Serial.println(value);
  digitalWrite(l, HIGH);
}

void initialise_pins(){
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  pinMode(A2, INPUT_PULLUP);
  pinMode(A3, INPUT_PULLUP);
  pinMode(A4, INPUT_PULLUP);
  pinMode(A5, INPUT_PULLUP);
  pinMode(A6, INPUT_PULLUP);
  pinMode(A7, INPUT_PULLUP);
  pinMode(A8, INPUT_PULLUP);
  pinMode(A9, INPUT_PULLUP);
  pinMode(A10, INPUT_PULLUP);
  pinMode(A11, INPUT_PULLUP);
  pinMode(A12, INPUT_PULLUP);
  pinMode(A13, INPUT_PULLUP);
  pinMode(A14, INPUT_PULLUP);
  pinMode(A15, INPUT_PULLUP);
  
  pinMode(UP_PIN, INPUT_PULLUP);
  pinMode(DOWN_PIN, INPUT_PULLUP);
  pinMode(KB_PIN, INPUT_PULLUP);
  pinMode(ESC_PIN, INPUT_PULLUP);
}

void initialise_features(){
  //Serial communication with the laptop
  Serial.begin(9600);
  Serial.println("Starting...");
  //tone start
  tone(pinBuzzer, 2000, 200);
  // set up the LCD's number of columns and rows: 
  //LCD init
  lcd.begin(16, 2);
  //EStop init
  initialiseEStop();
  //Initialise Menu
  menu.getRoot().add(miAnalog);
  miAnalog.addRight(miDigital).addRight(miSound);
  miAnalog.add(miASens1).addRight(miASens2);
  miDigital.add(miDSens1).addRight(miDSens2);
  miSound.add(miOn).addRight(miOff);
  menu.toRoot();
  lcd_default();
}

void readPins(){
  for(int i=0; i<=analogSensorsTotal; i++){
    //analogVal[i] = analogRead(analogIn[i]);
    //Serial.println(analogVal[i]);
  }
  //Serial.println(digitalRead(7));
  if(analogRead(A2)<20){
    //lcd.print("Laser triggered");
    //Serial.println("Laser triggered");
  }else{
    //lcd.print("Ready!");
    //Serial.println("ready");
  }
}

void display_refresh(){

}

void serial_write(){

}



void menuChanged(MenuChangeEvent changed){
  
  MenuItem newMenuItem=changed.to; //get the destination menu
  
  lcd.setCursor(0,0); 
  
  if(newMenuItem.getName()==menu.getRoot()){
      lcd_default();
  }else if(newMenuItem.getName()=="Analog sens"){
      lcd.print("  Analog sens   ");
  }else if(newMenuItem.getName()==">"){
      lcd.print("       ->       ");
  }else if(newMenuItem.getName()=="<"){
      lcd.print("       <-       ");
  }else if(newMenuItem.getName()=="Digital sens"){
      lcd.print("  Digital Sens  ");
  }else if(newMenuItem.getName()==">"){
      lcd.print("       ->       ");
  }else if(newMenuItem.getName()=="<"){
      lcd.print("       <-       ");
  }else if(newMenuItem.getName()=="Sound"){
      lcd.print("     Sound      ");
  }else if(newMenuItem.getName()=="on"){
      lcd.print("       on       ");
      EEPROM.write(3,1);
  }else if(newMenuItem.getName()=="off"){
      lcd.print("       off      ");
      EEPROM.write(3,0);
  }
  
}

void menuUsed(MenuUseEvent used){
  lcd.setCursor(0,0);  
  lcd.print("You used        ");
  //lcd.setCursor(0,1); 
  //lcd.print(used.item.getName());
  delay(3000);  
//  lcd.setCursor(0,0);  
//  lcd.print("Settings        ");
  menu.toRoot();  
  lcd_default();
}


void  readButtons(){  //read buttons status
  int reading;
  int buttonEnterState=HIGH;             
  int buttonEscState=HIGH;            
  int buttonLeftState=HIGH;           
  int buttonRightState=HIGH;          
  
  reading = digitalRead(KB_PIN);
  if (reading != lastButtonEnterState) {
    lastEnterDebounceTime = millis();
  } 
  if ((millis() - lastEnterDebounceTime) > debounceDelay) {
    buttonEnterState=reading;
    lastEnterDebounceTime=millis();
  }
  lastButtonEnterState = reading;
  reading = digitalRead(ESC_PIN);
  
 if (reading != lastButtonEscState) {
    lastEscDebounceTime = millis();
  } 
  
  if ((millis() - lastEscDebounceTime) > debounceDelay) {
    buttonEscState = reading;
    lastEscDebounceTime=millis();
  }
  lastButtonEscState = reading; 
  reading = digitalRead(DOWN_PIN);
  if (reading != lastButtonRightState) {
    lastRightDebounceTime = millis();
  } 
  
  if ((millis() - lastRightDebounceTime) > debounceDelay) {
    buttonRightState = reading;
   lastRightDebounceTime =millis();
  }
  lastButtonRightState = reading;                  
  reading = digitalRead(UP_PIN);
  if (reading != lastButtonLeftState) {
    lastLeftDebounceTime = millis();
  } 
  
  if ((millis() - lastLeftDebounceTime) > debounceDelay) {
    buttonLeftState = reading;
    lastLeftDebounceTime=millis();;
  }
  lastButtonLeftState = reading;  

  if (buttonEnterState==LOW){
    lastButtonPushed=KB_PIN;

  }else if(buttonEscState==LOW){
    lastButtonPushed=ESC_PIN;

  }else if(buttonRightState==LOW){
    lastButtonPushed=DOWN_PIN;

  }else if(buttonLeftState==LOW){
    lastButtonPushed=UP_PIN;

  }else{
    lastButtonPushed=0;
  }                  
}

void navigateMenus() {
  MenuItem currentMenu=menu.getCurrent();
  
  switch (lastButtonPushed){
    case KB_PIN:
      if(!(currentMenu.moveDown())){  //if the current menu has a child and has been pressed enter then menu navigate to item below
        menu.use();
      }else{  //otherwise, if menu has no child and has been pressed enter the current menu is used
        menu.moveDown();
       } 
      break;
    case ESC_PIN:
      menu.toRoot();  
      //TO DO JUMP ONE LEVEL UP menu.moveUp();
      break;
    case DOWN_PIN:
      menu.moveRight();
      break;      
    case UP_PIN:
      menu.moveLeft();
      break;      
  }
  
  lastButtonPushed=0; 
}

void lcd_default(){
  lcd.setCursor(0,0);  
  lcd.print("SloMoments Robo1");
  menurefreshed = true;
  lcd.setCursor(0,1);
    lcd.print("      ");
}

boolean estop = false;

void initialiseEStop(){
  pinMode(pinEstop, INPUT_PULLUP);
}

char emergencyStop(){
  if(digitalRead(pinEstop)){
    estop = false;
    return '0';
  }else{
    estop = true;
    return '1';
  }
}

void alarm(){
  lcd.setCursor(0,0);
  lcd.print("     PANIC!     ");
  lcd.setCursor(0,1);
  lcd.print(" EMERGENCY STOP ");
  tone(pinBuzzer, 5000);
  delay(50);
  tone(pinBuzzer, 1000);
  delay(50);
}

void initialiseAnalogDetection(){
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  pinMode(A2, INPUT_PULLUP);
  pinMode(A3, INPUT_PULLUP);
  pinMode(A4, INPUT_PULLUP);
  pinMode(A5, INPUT_PULLUP);
  pinMode(A6, INPUT_PULLUP);
  pinMode(A7, INPUT_PULLUP);
}

char AnalogDetection(){
  sensor1 = analogRead(sensorPin1);
  sensor2 = analogRead(sensorPin2);
  sensor3 = analogRead(sensorPin3);
  sensor4 = analogRead(sensorPin4);
  sensor5 = analogRead(sensorPin5);
  sensor6 = analogRead(sensorPin6);
  sensor7 = analogRead(sensorPin7);
  sensor8 = analogRead(sensorPin8);
 
  if(sensor1 > 1000)
    Serial.print("not s1 - ");
  else
    Serial.print("analog1 - ");
  
  if(sensor2 > 1000) 
    Serial.print("not s2 - "); 
  else
    Serial.print("analog2 - ");
  
  if(sensor3 > 1000) 
    Serial.print("not s3 - "); 
  else
    Serial.print("analog3 - ");
  
  if(sensor4 > 1000) 
    Serial.print("not s4 - "); 
  else
    Serial.print("analog4 - ");
    
  if(sensor5 > 1000) 
    Serial.print("not s5 - "); 
  else
    Serial.print("analog5 - ");
        
  if(sensor6 > 1000) 
    Serial.print("not s6 - "); 
  else
    Serial.print("analog6 - ");
    
  if(sensor7 > 1000) 
    Serial.print("not s7 - "); 
  else
    Serial.print("analog7 - ");
  
  if(sensor8 > 1000) 
    Serial.println("not s8"); 
  else
    Serial.println("analog8"); 
}


void initialiseAnalogTune(){
  pinMode(A8, INPUT_PULLUP);
  pinMode(A9, INPUT_PULLUP);
  pinMode(A10, INPUT_PULLUP);
  pinMode(A11, INPUT_PULLUP);
  pinMode(A12, INPUT_PULLUP);
  pinMode(A13, INPUT_PULLUP);
  pinMode(A14, INPUT_PULLUP);
  pinMode(A15, INPUT_PULLUP);
}

unsigned valori[50];
unsigned indexval[] = {0,0,0,0,0,0,0,0};
unsigned val_ant[] = {0,0,0,0,0,0,0,0};

int id = 0;
int nu = 0;
  
char analogTune(){

  int F1 = analogRead(12);
  if ((F1 != 274) && (F1 != 273) && (F1 != 272) && (F1 != 271) && (F1 != 270) && (F1 != 269) && (F1 != 268) && (F1 != 267) && (F1 != 266) && (F1 != 265) && (F1 != 264)){
    F1 = abs((analogRead(12)-14)*100/262 - 100);
  }else{
    F1=0;
  }
  Serial.print(F1);
  Serial.print(" - ");

/*----------------------ok------------------------*/  
  
  int F2 = analogRead(13);
  

  if ((F2 != 265) && (F2 != 264) && (F2 != 263)){
    F2 = abs((analogRead(13)-19)*100/265 - 100);
  }else{
    F2=0;
  }
    Serial.print(F2);
  Serial.print(" - ");
/*-----------------------ok-----------------------*/  
  
    int F3 = analogRead(14);
Serial.print(F3);
  Serial.print(" - ");      
  if ((F3 != 261) && (F3 != 260) && (F3 != 259)){
    F3 = abs((analogRead(14)-14)*100/255 - 100);
  }else{
    F3=0;
  }

  
  
    int F4 = analogRead(15);
    
  if ((F4 != 268) && (F4 != 267) && (F4 != 266)){
    F4 = abs((analogRead(15)-13)*100/260 - 100);
  }else{
    F4=0;
  }
  Serial.print(F4);
  Serial.print(" - ");
  
  
    int F5 = analogRead(8);
    Serial.print(F5);
  Serial.print(" - ");
  if ((F5 != 268) && (F5 != 267) && (F5 != 266)){
    F5 = abs((analogRead(8)-13)*100/261 - 100);
  }else{
    F5=0;
  }
  
    int F6 = analogRead(9);
    Serial.print(F6);
  Serial.print(" - ");
  if ((F6 != 268) && (F6 != 267) && (F6 != 266)){
    F6 = abs((analogRead(9)-12)*100/256 - 100);
  }else{
    F6=0;
  }
  
    int F7 = analogRead(10);
    Serial.print(F7);
  Serial.print(" - ");
  if ((F7 != 268) && (F7 != 267) && (F7 != 266)){
    F7 = abs((analogRead(10)-13)*100/258 - 100);
  }else{
    F7=0;
  }
   
    int F8 = analogRead(11);
     Serial.println(F8);
  if ((F8 != 268) && (F8 != 267) && (F8 != 266)){
    F8 = abs((analogRead(11)-13)*100/256 - 100);
  }else{
    F8=0;
  }
}