/* * Test Code 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 circle(float t, float &x, float &y) { static float amplitude = 0; static float inc = 0.001; static float angle = 0; amplitude += inc; angle += 0.1; if((inc > 0 && amplitude >= 1) || (inc < 0 && amplitude <= 0)) { inc = -inc; } x = -cos(angle) * amplitude; y = sin(angle) * amplitude; } void setup() { Serial.begin(9600); pinMode(U_PIN, OUTPUT); pinMode(D_PIN, OUTPUT); pinMode(L_PIN, OUTPUT); pinMode(R_PIN, OUTPUT); Serial.print("Computed spring constant: "); const float spring_K = computeSpringConstant(pull_test_vcc, pull_test_disp); Serial.println(spring_K); const float required_vcc = displacementToVoltage(displacement_amplitude); Serial.print("Target displacement amplitude: "); Serial.println(displacement_amplitude, 3); 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); } #define SPEED 1 void loop() { const float t = float(millis()) / 1000.0; float x, y; uint8_t v_x1, v_x2, v_y1, v_y2; circle(2 * PI * SPEED * t, x, y); x *= displacement_amplitude; y *= displacement_amplitude; getSolenoidVoltages(x, v_x1, v_x2); getSolenoidVoltages(y, v_y1, v_y2); delay(10); analogWrite(L_PIN, v_x1); analogWrite(R_PIN, v_x2); analogWrite(U_PIN, v_y1); analogWrite(D_PIN, v_y2); }