/* 
 * Lorenz Attractor for DIY Laser Steering Module for Arduino
 *
 * (c) 2018 Marcio Teixeira
 * 
 * The code in this page is free software: you can
 * redistribute it and/or modify it under the terms of the GNU
 * General Public License (GNU GPL) as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version.  The code is distributed WITHOUT ANY WARRANTY;
 * without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU GPL for more details.
 * 
 */

#define U_PIN  9
#define D_PIN  3
#define R_PIN 10
#define L_PIN 11

#define PWM_FREQ_31372Hz 0x01 // Sets the PWM frequency to 31372.55 Hz
#define PWM_FREQ_3921Hz  0x02 // Sets the PWM frequency to 3921.16 Hz
#define PWM_FREQ_980Hz   0x03 // Sets the PWM frequency to  980.39 Hz

/* The procedure for measuring the magnetic permeability of the core
 * is to measure the inductance of the solenoid L(x) with the armature
 * inserted at various depths, ranging from fully inserted (x=0) to
 * it being fully removed x=solenoid_len.
 * 
 * I obtained the following values:
 * 
 *    x      |  L meas   |  L(x) with U_r = 2.9
 *    0.0 mm |  19.8 mH  |       19.8 mH
 *    3.5 mm |  15.7 mH  |       15.2 mH
 *    7.0 mm |  11.1 mH  |       11.6 mH
 *   10.5 mm |   9.3 mH  |        8.9 mH
 *   14.0 mm |   9.1 mH  |        6.8 mH
 *   
 * I then used eq 15, P. Schimpf to find the value
 * of U_r that most closely matched my measurements,
 * aiming to maximize the closeness of the values
 * for x < 7 mm, since this is the region in which
 * the solenoid is operating.
 * 
 * I chose U_r = 2.9 in order to match the values 
 * I had measured.
 */
const float u_r           = 2.9;      // Magnetic permeability of armature, estimated based on L(x) measurements.

const float spring_K      = 10.41;    // Spring constant of hinges
const float R             = 52.1;     // Measured solenoid resistance of solenoid (ohms)
const float L_0           = 19.8e-3;  // Measured solenoid inductance with armature fully inserted (H)
const float L_len         =  9.1e-3;  // Measured solenoid inductance with armature fully removed (H)
const float solenoid_len  = 14e-3;    // Measured solenoid length (m)
const float spring_X0     = 3.0e-3;   // Measured position at which armature exerts no force on the hinges (m).
const float a             = log(u_r); // Eq. 16, P. Schimpf
const float solenoid_vcc  = 4.72;     // Power supply voltage

// To compute the spring constant, measure the displacement of the solenoid at a known voltage:

const float pull_test_vcc  = 9;        // Test voltage (V)
const float pull_test_disp = 0.002;    // Measured displacement at test voltage (m)

// Choose the following value to change the size of the rendered graphics.

const float displacement_amplitude = 0.45e-3; // Desired displacement amplitude.

float positionToVoltage(float x) {
  // Restoring force exerted by hinges (Hooke's Law) at desired x. 
  const float spring_F = -spring_K * (x - spring_X0);

  // Voltage such that the pulling force of the solenoid matches the
  // restoring force of the hinges
  //
  //    Based on equation 16, P. Schimpf.
  return sqrt(-2*R*R*(-spring_F)*solenoid_len/(a*L_0*exp(-a*x/solenoid_len)));
}

float displacementToVoltage(float disp) {
  return positionToVoltage(spring_X0 - disp);
}

// The procedure for finding the spring constant is to drive one of the
// solenoids of the dual-axis module with a test voltage and measure
// the displacement of the piston (in meters). This function will
// return the spring constant.

float computeSpringConstant(float V, float disp) {
  const float x = spring_X0 - disp;

  // The following is equation 17, P. Schimpf
  const float solenoid_F = (-V*V*a)/(2*R*R*solenoid_len)*L_0*exp(-a*x/solenoid_len);
  
  // Solve Hooke's Law for the spring constant:
  return -solenoid_F/disp;
}

// Allows adjustment of the PWM frequency. Higher values will allow
// for virtually silent operation at low speeds.
void setPWMTimerFrequencies(uint8_t frequency) {
  // Reference: http://playground.arduino.cc/Main/TimerPWMCheatsheet
  TCCR1B = (TCCR1B & 0b11111000) | frequency; // Set timer1 (pins 9 & 10) frequency
  TCCR2B = (TCCR2B & 0b11111000) | frequency; // Set timer2 (pins 3 & 11) frequency
}

void getSolenoidVoltages(float disp, uint8_t &v1, uint8_t &v2) {
  if(disp > 0) {
    v2 = min(255,displacementToVoltage( disp)*255/solenoid_vcc);
    v1 = 0;
  } else {
    v1 = min(255,displacementToVoltage(-disp)*255/solenoid_vcc);
    v2 = 0;
  }
}

void lorenz(float &x, float &y, float &z) {
  const float a = 10.0;
  const float b = 28.0;
  const float c = 8.0 / 3.0;

  const float dt = 0.0015;
  const float dx = dt * a * (y - x);
  const float dy = dt * (x * (b - z) - y);
  const float dz = dt * (x*y - c * z);
  x += dx;
  y += dy;
  z += dz;
}

void setup() {
  Serial.begin(9600);
  
  pinMode(U_PIN, OUTPUT);
  pinMode(D_PIN, OUTPUT);
  pinMode(L_PIN, OUTPUT);
  pinMode(R_PIN, OUTPUT);

  const float required_vcc = displacementToVoltage(displacement_amplitude);
  
  Serial.print("Target displacement amplitude: ");
  Serial.println(displacement_amplitude, 5);
  
  Serial.print("Required displacement voltage: ");
  Serial.println(required_vcc);

  Serial.print("Solenoid power supply: ");
  Serial.println(solenoid_vcc);

  if(required_vcc > solenoid_vcc) {
    Serial.println("The solenoid power supply is not able to sustain the target displacement, clipping will occur");
  }

  setPWMTimerFrequencies(PWM_FREQ_31372Hz);
}

void loop() {
  static float x = 0.1, y = 0, z = 0;
  uint8_t v_x1, v_x2, v_y1, v_y2;

  lorenz(x, y, z);
  
  getSolenoidVoltages((x    )/20 * displacement_amplitude, v_x1, v_x2);
  getSolenoidVoltages((y - 2)/20 * displacement_amplitude, v_y1, v_y2);

  analogWrite(L_PIN, v_x1);
  analogWrite(R_PIN, v_x2);
  analogWrite(U_PIN, v_y1);
  analogWrite(D_PIN, v_y2);
}