/*
 ______                                                      __      
/\__  _\       __                                 __        /\ \__   
\/_/\ \/ _ __ /\_\     __     ___   _____   _ __ /\_\    ___\ \ ,_\  
   \ \ \/\`'__\/\ \  /'_ `\  / __`\/\ '__`\/\`'__\/\ \ /' _ `\ \ \/  
    \ \ \ \ \/ \ \ \/\ \L\ \/\ \L\ \ \ \L\ \ \ \/ \ \ \/\ \/\ \ \ \_ 
     \ \_\ \_\  \ \_\ \____ \ \____/\ \ ,__/\ \_\  \ \_\ \_\ \_\ \__\
      \/_/\/_/   \/_/\/___L\ \/___/  \ \ \/  \/_/   \/_/\/_/\/_/\/__/
                       /\____/        \ \_\                          
                       \_/__/          \/_/                          

title: 		Arduino main brain
author: 	Trigoprint Ltd.
URL: 			http://www.trigoprint.com
revision: 0.15.3
tags: 		filament, robot, wheel, arduino, raspberry, gear, router, camera, quality, stepper motor
license: 	Attribution-NonCommercial-ShareAlike 4.0 International (CC BY-NC-SA 4.0)

You are free to:
Share — copy and redistribute the material in any medium or format
Adapt — remix, transform, and build upon the material
The licensor cannot revoke these freedoms as long as you follow the license terms.

Under the following terms:

Attribution — You must give appropriate credit, provide a link to the license, 
and indicate if changes were made. You may do so in any reasonable manner, 
but not in any way that suggests the licensor endorses you or your use.

NonCommercial — You may not use the material for commercial purposes.

ShareAlike — If you remix, transform, or build upon the material, you must 
distribute your contributions under the same license as the original.

No additional restrictions — You may not apply legal terms or technological 
measures that legally restrict others from doing anything the license permits.

Notices:

You do not have to comply with the license for elements of the material in the 
public domain or where your use is permitted by an applicable exception or limitation.
No warranties are given. The license may not give you all of the permissions 
necessary for your intended use. For example, other rights such as publicity, 
privacy, or moral rights may limit how you use the material.
*/
// CONFIG ----------------------------------------------------------------------

// User Settings 

// distance to go in cm, should be smaller than circumference of spool
int distance_to_measure = 50;

// spool diameter in cm
float spool_dia = 19.6;

// spool width in cm
float spool_width = 7.3;
// gear ratio
float gear_ratio = 36;

// weight of one spool in gramm
float filament_weigth = 1000;

//  specific density
// ABS 1.04 g/cm^3
// PLA 1.25 g/cm^3
// Nylon 1.134 g/cm^3
float filament_dens = 1.134;

// Filament Diameter in mm
float filament_dia = 2.85;

// enable camera > true or false
boolean camera_enable = true;

// ###############################################################
// Stepper for Drive
// Stepsize 
// 1 turn = 200 steps (1.8° Stepper with Full Step, 0.9° = 400) 
int oneturn_steps = 200;

// Stepsize for Drive
// possible values
// 1, 2, 4, 8, 16, 32
int stepsize_drive = 1; // 1.8° Steppermotor
// ###############################################################

// ###############################################################
// Stepper for Router
// M5 with  1/1 step = 250 steps/mm
int steps_mm = 250;
int step_size = 1;

// go to position after init in mm
int start_distance_mm = 4;
// ###############################################################

// delay with included software on a raspberry PI Modell B+
float camera_delay = 3000.0;
// delay with pil 4500

// User Settings END


// calculated settings no user change needed here
float pi_nr = 3.14159265359;
float spool_scope = (2*pi_nr*(spool_dia/2));
// filament feeded in one turn
float one_turn = spool_scope/gear_ratio;
// steps to move for next shot
int steps_to_go = (oneturn_steps*stepsize_drive)/(one_turn/distance_to_measure);
//counter for shots
int runstep = 1;
// active status
boolean running = 0;
boolean active = 0;
float filament_lenght1mm_vol_cm3 = (2*pi_nr*(filament_dia/2)*1/100);
float filament_cm3 = filament_weigth/filament_dens;
float filament_cm = filament_cm3/filament_lenght1mm_vol_cm3;
int shots_to_end = filament_cm/distance_to_measure;
float spool_width_count = (spool_width*10)/filament_dia;
//set time for hardware
float shot_time = (shots_to_end*(camera_delay+4000))/1000/60;
float filament_meter = shots_to_end*distance_to_measure/100;
float filament_rills = (filament_meter*100)/spool_scope;
float filament_layers = filament_rills/spool_width_count;
int max_distance_mm = spool_width*10;
int steps_real = steps_mm/step_size;
int travel_distance_max_steps = max_distance_mm*steps_real;
boolean router_direction = true;
int router_position = 0;
int router_travel_steps = distance_to_measure/spool_scope*filament_dia*steps_real;
// CONFIG END ------------------------------------------------------------------

#include <Wire.h>
#include <AccelStepper.h>
// pin 1 = Step input
// pin 2 = Direction input
// direction not used d10
AccelStepper stepper(AccelStepper::DRIVER, 5, 10); 
AccelStepper stepper1(AccelStepper::DRIVER, 9, 8);
// hardware limit
int max_speed = 500;
// motor speed max 500
int norm_speed = 500;
int Acceleration = 500;
boolean motor_active = 0;
// pin for enable drive
const int enable_drive = 6;
// pin for enable router
const int enable_router = 7;
// button on pin3
const int buttonPin = 3;
int buttonState = 0;
int lastButtonState = LOW;
long lastDebounceTime = 0; // the last time the output pin was toggled
long debounceDelay = 50; // the debounce time; increase if the output flickers
// endstop on pin4
const int buttonPin_endstop = 4;
int buttonState_endstop = 0;
int lastButtonState_endstop = LOW;
// I2C Setup needed for camera
  #define SLAVE_ADDRESS 0x04
  byte number = 0;
  int state = 0;
  boolean answer = 1;

// ----------------------------------------------------------------------
void setup() {
Serial.begin(9600); // start serial for output

//Buttons
pinMode(13, OUTPUT);  // onboard LED for activity
pinMode(buttonPin, INPUT);
pinMode(buttonPin_endstop, INPUT);
// steppers
pinMode(enable_drive, OUTPUT);
pinMode(enable_router, OUTPUT);

// initialize i2c as slave
// On the Arduino Uno, the I2C pins are pins A4 (SDA) and A5 (SCL)
// On the Arduino Mega, they are 20 (SDA), 21 (SCL)
if (camera_enable == true){
  Wire.begin(SLAVE_ADDRESS);
  // define callbacks for i2c communication
  Wire.onReceive(receiveData);
  Wire.onRequest(sendData);
}

//set stepper speed
stepper.setMaxSpeed(max_speed);
stepper.setSpeed(norm_speed);
stepper.setAcceleration(Acceleration);
stepper.setCurrentPosition(0);

//set stepper speed
stepper1.setMaxSpeed(max_speed);
stepper1.setSpeed(norm_speed);
stepper1.setAcceleration(Acceleration);
stepper1.setCurrentPosition(0);

///enable steppers must be low
digitalWrite(enable_drive, LOW);
digitalWrite(enable_router, LOW);

//router init
buttonState_endstop = digitalRead(buttonPin_endstop);
Serial.println(buttonState_endstop);
while (buttonState_endstop == 1)
  {
    stepper1.runToNewPosition(-250);
    stepper1.run();
    buttonState_endstop = digitalRead(buttonPin_endstop);      
    stepper1.setCurrentPosition(0);
    Serial.print("waiting for Stepper: ");
    Serial.println(buttonState_endstop);      
  }

//Stop the stepper motor
stepper1.stop(); 
stepper1.setCurrentPosition(0);
      
// go to init position
stepper1.runToNewPosition(start_distance_mm*steps_real);
stepper1.run();
stepper1.setCurrentPosition(0);

Serial.println("Wheeler ready!");
Serial.print("travel distance per shot cm: ");
Serial.println(distance_to_measure);
Serial.print("total meter: ");
Serial.println(filament_meter);
Serial.print("spool total rills: ");
Serial.println(filament_rills);
Serial.print("spool total layers: ");
Serial.println(filament_layers);
Serial.print("spool scope cm: ");
Serial.println(spool_scope);
Serial.print("spool one turn (gear ratio) cm: ");
Serial.println(one_turn);
Serial.print("steps one shot: ");
Serial.println(steps_to_go);
Serial.print("shots for this spool: ");
Serial.println(shots_to_end);
Serial.print("spool width count: ");
Serial.println(spool_width_count);
Serial.print("estaminated time in minutes: ");
Serial.println(shot_time);
Serial.print("camera delay ms: ");
Serial.println(camera_delay);
}
// ----------------------------------------------------------------------
void loop() {

//read the start button
int reading = digitalRead(buttonPin);
if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }
if ((millis() - lastDebounceTime) > debounceDelay) {
    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == HIGH) {
        //Serial.print("button pressed");                
        if (running == 0){
          active = 1;
          running = 1;
          Serial.println("active");
        }
        else{
          active = 0;
          running = 0;
          Serial.println("stopped");
        }
      }
    }
  }
lastButtonState = reading;

//endstop
/*
buttonState_endstop = digitalRead(buttonPin_endstop);
if (buttonState_endstop == 1){
  active = 0;
  running = 0;
}
*/

if (active == 1){
if (shots_to_end+1 > runstep){
  if (motor_active == 0){
  run_motor();
  runstep = runstep+1;
  }
}
}
//stepper
stepper.run();
stepper1.run();

}//end of loop
// ----------------------------------------------------------------------

// functions 

void run_motor(){
  motor_active = 1;
  Serial.print("current shot: ");
  Serial.print(runstep);
  Serial.print(" / ");
  Serial.print(shots_to_end);

if (camera_enable == true){
  //make the shot
  makepic();
  delay (camera_delay);
  }

  // filament feeder
  stepper.runToNewPosition(steps_to_go);
  stepper.run();
  stepper.setCurrentPosition(0);
  
  // router
  if (router_position >= travel_distance_max_steps){    
    router_direction = false;
    stepper1.setCurrentPosition(travel_distance_max_steps);
  }
      
  if (router_position <= 0){
    router_direction = true;
    stepper1.setCurrentPosition(0);
  }
  
  if (router_direction == true){
    router_position = router_position + router_travel_steps;
  }
  else{
    router_position = router_position - router_travel_steps;
  }
  
  Serial.print(" - direction : ");
  Serial.print(router_direction);
  
  Serial.print(" position : ");
  Serial.println(router_position);
  
  stepper1.runToNewPosition(router_position);
  stepper1.run();
  
  motor_active = 0;
}

void makepic () {
  number = answer;
  sendData();  
  delay (100);
  number = 0;
}

// callback for received data
void receiveData(int byteCount){

while(Wire.available()) {
  number = Wire.read();
  Serial.print("data received: ");
  Serial.println(number);
  stepper.setSpeed(number);

    if (number == 1){

      if (state == 0){
      state = 1;
      }
    else{
      state = 0;
      }
    }
  } // end of while 
} // end of function

// callback for sending data
void sendData(){
  Wire.write(number);
}