/
laser.cpp
191 lines (171 loc) · 6.33 KB
/
laser.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
/*
laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1
Copyright (c) 2013 Timothy Schmidt. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "laser.h"
#include "Configuration.h"
#include "ConfigurationStore.h"
#include "pins.h"
#include <avr/interrupt.h>
#include <Arduino.h>
#include "Marlin.h"
laser_t laser;
void timer3_init(int pin) {
pinMode(pin, OUTPUT);
analogWrite(pin, 1); // let Arduino setup do it's thing to the PWM pin
TCCR3B = 0x00; // stop Timer4 clock for register updates
TCCR3A = 0x82; // Clear OC3A on match, fast PWM mode, lower WGM3x=14
ICR3 = labs(F_CPU / LASER_PWM); // clock cycles per PWM pulse
OCR3A = labs(F_CPU / LASER_PWM) - 1; // ICR3 - 1 force immediate compare on next tick
TCCR3B = 0x18 | 0x01; // upper WGM4x = 14, clock sel = prescaler, start running
noInterrupts();
TCCR3B &= 0xf8; // stop timer, OC3A may be active now
TCNT3 = labs(F_CPU / LASER_PWM); // force immediate compare on next tick
ICR3 = labs(F_CPU / LASER_PWM); // set new PWM period
TCCR3B |= 0x01; // start the timer with proper prescaler value
interrupts();
}
void timer4_init(int pin) {
pinMode(pin, OUTPUT);
analogWrite(pin, 1); // let Arduino setup do it's thing to the PWM pin
TCCR4B = 0x00; // stop Timer4 clock for register updates
TCCR4A = 0x82; // Clear OC4A on match, fast PWM mode, lower WGM4x=14
ICR4 = labs(F_CPU / LASER_PWM); // clock cycles per PWM pulse
OCR4A = labs(F_CPU / LASER_PWM) - 1; // ICR4 - 1 force immediate compare on next tick
TCCR4B = 0x18 | 0x01; // upper WGM4x = 14, clock sel = prescaler, start running
noInterrupts();
TCCR4B &= 0xf8; // stop timer, OC4A may be active now
TCNT4 = labs(F_CPU / LASER_PWM); // force immediate compare on next tick
ICR4 = labs(F_CPU / LASER_PWM); // set new PWM period
TCCR4B |= 0x01; // start the timer with proper prescaler value
interrupts();
}
void laser_init()
{
// Initialize timers for laser intensity control
#if LASER_CONTROL == 1
if (LASER_FIRING_PIN == 2 || LASER_FIRING_PIN == 3 || LASER_FIRING_PIN == 5) timer3_init(LASER_FIRING_PIN);
if (LASER_FIRING_PIN == 6 || LASER_FIRING_PIN == 7 || LASER_FIRING_PIN == 8) timer4_init(LASER_FIRING_PIN);
#endif
#if LASER_CONTROL == 2
if (LASER_INTENSITY_PIN == 2 || LASER_INTENSITY_PIN == 3 || LASER_INTENSITY_PIN == 5) timer3_init(LASER_INTENSITY_PIN);
if (LASER_INTENSITY_PIN == 6 || LASER_INTENSITY_PIN == 7 || LASER_INTENSITY_PIN == 8) timer4_init(LASER_INTENSITY_PIN);
#endif
#ifdef LASER_PERIPHERALS
digitalWrite(LASER_PERIPHERALS_PIN, HIGH); // Laser peripherals are active LOW, so preset the pin
pinMode(LASER_PERIPHERALS_PIN, OUTPUT);
digitalWrite(LASER_PERIPHERALS_STATUS_PIN, HIGH); // Set the peripherals status pin to pull-up.
pinMode(LASER_PERIPHERALS_STATUS_PIN, INPUT);
#endif // LASER_PERIPHERALS
// initialize state to some sane defaults
laser.intensity = 100.0;
laser.ppm = 0.0;
laser.duration = 0;
laser.status = LASER_OFF;
laser.firing = LASER_OFF;
laser.mode = CONTINUOUS;
laser.last_firing = 0;
laser.diagnostics = false;
laser.time = 0;
#ifdef LASER_RASTER
laser.raster_aspect_ratio = LASER_RASTER_ASPECT_RATIO;
laser.raster_mm_per_pulse = LASER_RASTER_MM_PER_PULSE;
laser.raster_direction = 1;
#endif // LASER_RASTER
#ifdef MUVE_Z_PEEL
laser.peel_distance = 2.0;
laser.peel_speed = 2.0;
laser.peel_pause = 0.0;
#endif // MUVE_Z_PEEL
laser_extinguish();
}
void laser_fire(int intensity = 100.0){
laser.firing = LASER_ON;
laser.last_firing = micros(); // microseconds of last laser firing
if (intensity > 100.0) intensity = 100.0; // restrict intensity between 0 and 100
if (intensity < 0) intensity = 0;
pinMode(LASER_FIRING_PIN, OUTPUT);
#if LASER_CONTROL == 1
analogWrite(LASER_FIRING_PIN, labs((intensity / 100.0)*(F_CPU / LASER_PWM)));
#endif
#if LASER_CONTROL == 2
analogWrite(LASER_INTENSITY_PIN, labs((intensity / 100.0)*(F_CPU / LASER_PWM)));
digitalWrite(LASER_FIRING_PIN, HIGH);
#endif
if (laser.diagnostics) {
SERIAL_ECHOLN("Laser fired");
}
}
void laser_extinguish(){
if (laser.firing == LASER_ON) {
laser.firing = LASER_OFF;
// Engage the pullup resistor for TTL laser controllers which don't turn off entirely without it.
digitalWrite(LASER_FIRING_PIN, LOW);
laser.time += millis() - (laser.last_firing / 1000);
if (laser.diagnostics) {
SERIAL_ECHOLN("Laser extinguished");
}
}
}
void laser_set_mode(int mode){
switch(mode){
case 0:
laser.mode = CONTINUOUS;
return;
case 1:
laser.mode = PULSED;
return;
case 2:
laser.mode = RASTER;
return;
}
}
#ifdef LASER_PERIPHERALS
bool laser_peripherals_ok(){
return !digitalRead(LASER_PERIPHERALS_STATUS_PIN);
}
void laser_peripherals_on(){
digitalWrite(LASER_PERIPHERALS_PIN, LOW);
if (laser.diagnostics) {
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Laser Peripherals Enabled");
}
}
void laser_peripherals_off(){
if (!digitalRead(LASER_PERIPHERALS_STATUS_PIN)) {
digitalWrite(LASER_PERIPHERALS_PIN, HIGH);
if (laser.diagnostics) {
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Laser Peripherals Disabled");
}
}
}
void laser_wait_for_peripherals() {
unsigned long timeout = millis() + LASER_PERIPHERALS_TIMEOUT;
if (laser.diagnostics) {
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Waiting for peripheral control board signal...");
}
while(!laser_peripherals_ok()) {
if (millis() > timeout) {
if (laser.diagnostics) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Peripheral control board failed to respond");
}
Stop();
break;
}
}
}
#endif // LASER_PERIPHERALS