Compare commits
No commits in common. "can/decoupled" and "main" have entirely different histories.
can/decoup
...
main
1
.gitignore
vendored
1
.gitignore
vendored
|
|
@ -1 +0,0 @@
|
|||
.clangd
|
||||
54
README.md
54
README.md
|
|
@ -3,12 +3,9 @@
|
|||
Code de la tourelle de pointage laser du projet bouchons EDF.
|
||||
|
||||
> [!WARNING]
|
||||
> Code de débogage uniquement.
|
||||
> Code de debuggage uniquement.
|
||||
|
||||
> [!IMPORTANT]
|
||||
> Nouvelle mécanique, les axes H/x et V/y sont découplés.
|
||||
|
||||
## Usage
|
||||
# Usage
|
||||
|
||||
Compile through Arduino IDE or equivalent.
|
||||
|
||||
|
|
@ -16,51 +13,6 @@ Compile through Arduino IDE or equivalent.
|
|||
> Add [kissStepper](https://github.com/risitt/kissStepper) to your project
|
||||
> librairies.
|
||||
|
||||
## Conventions
|
||||
|
||||
- **x** is the horizontal axis centered at 0 and going from left (min value) to
|
||||
right (max value).
|
||||
- **y** is the vertical axis centered at 0 and going from bottom (min value) to
|
||||
top (max value).
|
||||
- **z** is the distance between the turret and the target.
|
||||
|
||||
```mermaid
|
||||
xychart-beta
|
||||
title "Turret coordinate system"
|
||||
x-axis "X axis [m]" -3 --> 3
|
||||
y-axis "Y axis [m]" -3 --> 3
|
||||
bar [-4]
|
||||
```
|
||||
|
||||
## Development
|
||||
|
||||
Required dependencies if not using
|
||||
[Arduino IDE](https://www.arduino.cc/en/software/):
|
||||
|
||||
- [Arduino CLI](https://docs.arduino.cc/arduino-cli/):
|
||||
- Install:
|
||||
```sh
|
||||
# linux
|
||||
curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | sh
|
||||
|
||||
# macos
|
||||
brew update
|
||||
brew install arduino-cli
|
||||
|
||||
# windows
|
||||
winget install --id=ArduinoSA.CLI -e
|
||||
```
|
||||
- Configure:
|
||||
```sh
|
||||
arduino-cli config init
|
||||
arduino-cli core update-index
|
||||
arduino-cli core install arduino:avr
|
||||
```
|
||||
|
||||
If you use `clangd` as lsp run `python ./generate_clangd.py` to load arduino
|
||||
config and libraries.
|
||||
|
||||
## Contributing
|
||||
|
||||
Before `git commit`, run `git clang-format --staged` to format staged files and
|
||||
then `git add` to commit formatting.
|
||||
Before `git commit`, run `git clang-format` to format stagged files.
|
||||
|
|
|
|||
35
config.h
35
config.h
|
|
@ -1,8 +1,6 @@
|
|||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
#include "math.h"
|
||||
|
||||
#define BAUDRATE (9600)
|
||||
|
||||
// Pins
|
||||
|
|
@ -16,33 +14,16 @@
|
|||
#define PIN_PD2 (2)
|
||||
#define PIN_MOSI (11)
|
||||
|
||||
#define PIN_Y_PULSE (PIN_PB0)
|
||||
#define PIN_Y_ENABLE (PIN_PD6)
|
||||
#define PIN_Y_DIRECTION (PIN_PD7)
|
||||
#define PIN_Y_HOME (PIN_PD3)
|
||||
#define PIN_X_PULSE (PIN_PB0)
|
||||
#define PIN_X_ENABLE (PIN_PD6)
|
||||
#define PIN_X_DIRECTION (PIN_PD7)
|
||||
#define PIN_X_HOME (PIN_PD3)
|
||||
|
||||
#define PIN_X_PULSE (PIN_PB2)
|
||||
#define PIN_X_ENABLE (PIN_PB1)
|
||||
#define PIN_X_DIRECTION (PIN_MOSI)
|
||||
#define PIN_X_HOME (PIN_PD4)
|
||||
#define PIN_Y_PULSE (PIN_PB2)
|
||||
#define PIN_Y_ENABLE (PIN_PB1)
|
||||
#define PIN_Y_DIRECTION (PIN_MOSI)
|
||||
#define PIN_Y_HOME (PIN_PD4)
|
||||
|
||||
#define PIN_LASER (PIN_PD2)
|
||||
|
||||
// Stepper ratio [step/rad]
|
||||
#define STEP_RATIO_X (6432 / M_PI)
|
||||
#define STEP_RATIO_Y (6432 / M_PI)
|
||||
|
||||
// Offset between home and zero [step]
|
||||
#define ZERO_OFFSET_X (0)
|
||||
#define ZERO_OFFSET_Y (2500)
|
||||
|
||||
// Offset between turret and lidar [cm]
|
||||
// Distance between the lidar optical center and the vertical laser (should be 0
|
||||
// unless mechanical constraints)
|
||||
#define OFFSET_X (0)
|
||||
// Distance between the lidar optical center and the horizontal laser
|
||||
#define OFFSET_Y (21)
|
||||
// Distance between the surface and the turret motors axis
|
||||
#define OFFSET_Z (166)
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,48 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import platform
|
||||
import yaml
|
||||
|
||||
def write_clangd_file(flags):
|
||||
clangd_config = {
|
||||
"CompileFlags": {
|
||||
"Add": flags
|
||||
}
|
||||
}
|
||||
|
||||
with open(".clangd", "w") as f:
|
||||
yaml.dump(clangd_config, f, default_flow_style=False)
|
||||
|
||||
def main():
|
||||
arduino_base_path = os.path.expanduser("~/AppData/Local") if platform.system() == "Windows" else os.path.expanduser('~')
|
||||
|
||||
flags = [
|
||||
"-x", "c++",
|
||||
"-std=gnu++11",
|
||||
"-fpermissive",
|
||||
"-fno-exceptions",
|
||||
"-ffunction-sections",
|
||||
"-fdata-sections",
|
||||
"-fno-threadsafe-statics",
|
||||
"-Wno-error=narrowing",
|
||||
"-flto",
|
||||
"-E",
|
||||
"-CC",
|
||||
"-D__AVR__",
|
||||
"-D__AVR_ATmega328P__",
|
||||
"-DF_CPU=16000000L",
|
||||
"-DARDUINO=10607",
|
||||
"-DARDUINO_AVR_UNO",
|
||||
"-DARDUINO_ARCH_AVR",
|
||||
"-I./include",
|
||||
f"-I{arduino_base_path}/Arduino15/packages/arduino/hardware/avr/1.8.6/cores/arduino",
|
||||
f"-I{arduino_base_path}/Arduino15/packages/arduino/hardware/avr/1.8.6/variants/standard",
|
||||
f"-I{arduino_base_path}/Arduino15/packages/arduino/tools/avr-gcc/7.3.0-atmel3.6.1-arduino7/avr/include",
|
||||
f"-I{arduino_base_path}/Arduino15/packages/arduino/tools/avr-gcc/7.3.0-atmel3.6.1-arduino7/lib/gcc/avr/7.3.0/include",
|
||||
]
|
||||
|
||||
write_clangd_file(flags)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,312 +0,0 @@
|
|||
/*
|
||||
kissStepper - a lightweight library for the Easy Driver, Big Easy Driver, Allegro stepper motor drivers and others that use a Step/Dir interface
|
||||
Written by Rylee Isitt. September 21, 2015
|
||||
License: GNU Lesser General Public License (LGPL) V2.1
|
||||
|
||||
Despite the existence of several excellent libraries for driving stepper motors, I created this one to fulfill the following needs:
|
||||
- Simplicity
|
||||
- Handling of enable, step, and dir pins
|
||||
- Based around an external loop
|
||||
- Approximately linear acceleration using a fast algorithm
|
||||
- High step frequency (or reasonably so, given the overhead involved)
|
||||
- Use AVR/ARM libraries and port access to increase performance while keeping the API Arduino-friendly
|
||||
- Teensy (Teensyduino) compatibility
|
||||
|
||||
Acceleration approximation math is based on Aryeh Eiderman's "Real Time Stepper Motor Linear Ramping Just by Addition and Multiplication", available at http://hwml.com/LeibRamp.pdf
|
||||
*/
|
||||
|
||||
#ifndef kissStepper_H
|
||||
#define kissStepper_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// determine port register size
|
||||
#if defined(__AVR__) || defined(__avr__)
|
||||
typedef uint8_t regint;
|
||||
#elif defined(TEENSYDUINO)
|
||||
#if defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB1286__) || defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
|
||||
typedef uint8_t regint;
|
||||
#else
|
||||
typedef uint32_t regint;
|
||||
#endif
|
||||
#else
|
||||
typedef uint32_t regint;
|
||||
#endif
|
||||
|
||||
|
||||
// the order of enums allows some simple tests:
|
||||
// if > STATE_STARTING, motor is in motion
|
||||
// if > STATE_RUN, motor is accelerating or decelerating
|
||||
enum kissState_t: uint8_t
|
||||
{
|
||||
STATE_STOPPED = 0,
|
||||
STATE_STARTING = 1,
|
||||
STATE_RUN = 2,
|
||||
STATE_ACCEL = 3,
|
||||
STATE_DECEL = 4
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// kissStepper without acceleration
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
|
||||
class kissStepperNoAccel
|
||||
{
|
||||
public:
|
||||
kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
|
||||
kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
|
||||
~kissStepperNoAccel(void) {};
|
||||
|
||||
bool prepareMove(int32_t target);
|
||||
kissState_t move(void);
|
||||
void stop(void);
|
||||
|
||||
uint16_t getCurSpeed(void)
|
||||
{
|
||||
if (m_kissState == STATE_RUN)
|
||||
return m_maxSpeed;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
kissState_t getState(void)
|
||||
{
|
||||
return m_kissState;
|
||||
}
|
||||
int32_t getPos(void)
|
||||
{
|
||||
if (m_forwards)
|
||||
return m_pos + m_distMoved;
|
||||
else
|
||||
return m_pos - m_distMoved;
|
||||
}
|
||||
bool isEnabled(void)
|
||||
{
|
||||
return m_enabled;
|
||||
}
|
||||
bool isMovingForwards(void)
|
||||
{
|
||||
return m_forwards;
|
||||
}
|
||||
void begin(void);
|
||||
void enable(void);
|
||||
void disable(void);
|
||||
|
||||
void setPos(int32_t pos)
|
||||
{
|
||||
if (m_kissState == STATE_STOPPED)
|
||||
m_pos = constrain(pos, m_reverseLimit, m_forwardLimit);
|
||||
}
|
||||
int32_t getTarget(void)
|
||||
{
|
||||
if (m_kissState == STATE_STOPPED)
|
||||
return m_pos;
|
||||
else if (m_forwards)
|
||||
return m_pos + m_distTotal;
|
||||
else
|
||||
return m_pos - m_distTotal;
|
||||
}
|
||||
uint32_t getDistRemaining(void)
|
||||
{
|
||||
return m_distTotal - m_distMoved;
|
||||
}
|
||||
void setForwardLimit(int32_t forwardLimit)
|
||||
{
|
||||
m_forwardLimit = forwardLimit;
|
||||
}
|
||||
void setReverseLimit(int32_t reverseLimit)
|
||||
{
|
||||
m_reverseLimit = reverseLimit;
|
||||
}
|
||||
int32_t getForwardLimit(void)
|
||||
{
|
||||
return m_forwardLimit;
|
||||
}
|
||||
int32_t getReverseLimit(void)
|
||||
{
|
||||
return m_reverseLimit;
|
||||
}
|
||||
void setMaxSpeed(uint16_t maxSpeed)
|
||||
{
|
||||
if (m_kissState == STATE_STOPPED) m_maxSpeed = maxSpeed;
|
||||
}
|
||||
uint16_t getMaxSpeed(void)
|
||||
{
|
||||
return m_maxSpeed;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setDir(bool forwards)
|
||||
{
|
||||
m_forwards = forwards;
|
||||
digitalWrite(PIN_DIR, forwards == m_invertDir);
|
||||
}
|
||||
void updatePos(void)
|
||||
{
|
||||
if (m_forwards)
|
||||
m_pos += m_distMoved;
|
||||
else
|
||||
m_pos -= m_distMoved;
|
||||
m_distMoved = 0;
|
||||
}
|
||||
static const uint32_t ONE_SECOND = 1000000UL;
|
||||
static const uint8_t PULSE_WIDTH_US = 2; // desired width of step pulse (high) in us
|
||||
static const int32_t DEFAULT_FORWARD_LIMIT = 2147483647L;
|
||||
static const int32_t DEFAULT_REVERSE_LIMIT = -2147483648L;
|
||||
static const uint16_t DEFAULT_SPEED = 1600;
|
||||
static const uint16_t INTERVAL_CORRECTION_INCREMENT = 255;
|
||||
|
||||
int32_t m_forwardLimit;
|
||||
int32_t m_reverseLimit;
|
||||
uint16_t m_maxSpeed;
|
||||
|
||||
const uint8_t PIN_DIR;
|
||||
const uint8_t PIN_STEP;
|
||||
const uint8_t PIN_ENABLE;
|
||||
|
||||
kissState_t m_kissState;
|
||||
uint32_t m_distTotal, m_distMoved;
|
||||
bool m_forwards;
|
||||
int32_t m_pos;
|
||||
|
||||
const regint m_stepBit;
|
||||
regint volatile * const m_stepOut;
|
||||
|
||||
uint32_t m_stepIntervalWhole;
|
||||
uint16_t m_stepIntervalRemainder;
|
||||
uint16_t m_stepIntervalCorrectionCounter;
|
||||
bool m_enabled;
|
||||
uint32_t m_lastStepTime;
|
||||
bool m_invertDir;
|
||||
bool m_init;
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// kissStepper WITH acceleration
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
|
||||
class kissStepper: public kissStepperNoAccel
|
||||
{
|
||||
public:
|
||||
kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
|
||||
kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
|
||||
~kissStepper(void) {};
|
||||
bool prepareMove(int32_t target);
|
||||
kissState_t move(void);
|
||||
void stop(void);
|
||||
|
||||
uint16_t getCurSpeed(void)
|
||||
{
|
||||
if (m_kissState == STATE_RUN)
|
||||
return m_maxSpeed;
|
||||
else if (m_kissState > STATE_STARTING)
|
||||
{
|
||||
uint32_t curSpeed = ONE_SECOND / m_stepIntervalWhole;
|
||||
if (curSpeed > m_maxSpeed) curSpeed = m_maxSpeed;
|
||||
return curSpeed;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
void decelerate(void);
|
||||
uint32_t calcMaxAccelDist(void)
|
||||
{
|
||||
if (m_accel > 0)
|
||||
return ((uint32_t)m_maxSpeed * m_maxSpeed) / (2UL * m_accel);
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
uint32_t getAccelDist(void)
|
||||
{
|
||||
return m_distAccel;
|
||||
}
|
||||
uint32_t getRunDist(void)
|
||||
{
|
||||
return m_distRun - m_distAccel;
|
||||
}
|
||||
uint32_t getDecelDist(void)
|
||||
{
|
||||
return m_distTotal - m_distRun;
|
||||
}
|
||||
void setAccel(uint16_t accel)
|
||||
{
|
||||
if (m_kissState == STATE_STOPPED) m_accel = accel;
|
||||
}
|
||||
uint16_t getAccel(void)
|
||||
{
|
||||
return m_accel;
|
||||
}
|
||||
uint16_t getTopSpeed(void);
|
||||
|
||||
protected:
|
||||
|
||||
static const uint16_t DEFAULT_ACCEL = 1600;
|
||||
uint32_t m_distAccel, m_distRun;
|
||||
uint32_t m_topSpeedStepInterval;
|
||||
uint32_t m_minSpeedStepInterval;
|
||||
float m_stepInterval;
|
||||
float m_constMult;
|
||||
uint16_t m_accel;
|
||||
|
||||
private:
|
||||
|
||||
/*
|
||||
----------------------------------------------------------------------------------------------------
|
||||
|
||||
To strike a balance between accuracy and performance, this library uses a set of approximations
|
||||
for calculating stepInterval when accelerating/decelerating. Although this does use floating point
|
||||
math, it is a drastic improvement over exact calculations and better than anything else I've tried.
|
||||
|
||||
There is probably room for further improvement (fixed point or integer math?) but this is good enough.
|
||||
|
||||
exact:
|
||||
stepInterval = ONE_SECOND / newSpeed
|
||||
curSpeed = ONE_SECOND / stepInterval
|
||||
newSpeed = sqrt(curSpeed^2 + 2a)
|
||||
stepInterval = ONE_SECOND / sqrt(curSpeed^2 + 2a)
|
||||
|
||||
approximations:
|
||||
constMult = accel / (ONE_SECOND * ONE_SECOND)
|
||||
q = constMult*stepInterval*stepInterval
|
||||
set q to negative if accelerating
|
||||
|
||||
good precision, fast: stepInterval *= 1.0 + q
|
||||
better precision, slower: stepInterval *= 1.0 + q + q*q
|
||||
best precision, slowest: stepInterval *= 1.0 + q + 1.5*q*q
|
||||
|
||||
----------------------------------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
float accelStep(float stepInterval, float constMult)
|
||||
{
|
||||
float newStepInterval;
|
||||
float q = -constMult*stepInterval*stepInterval;
|
||||
newStepInterval = stepInterval * (1.0 + q);
|
||||
// newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
|
||||
// newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
|
||||
if (newStepInterval < m_topSpeedStepInterval) newStepInterval = m_topSpeedStepInterval;
|
||||
return newStepInterval;
|
||||
}
|
||||
|
||||
float decelStep(float stepInterval, float constMult)
|
||||
{
|
||||
float newStepInterval;
|
||||
float q = constMult*stepInterval*stepInterval;
|
||||
newStepInterval = stepInterval * (1.0 + q);
|
||||
// newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
|
||||
// newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
|
||||
if (newStepInterval > m_minSpeedStepInterval) newStepInterval = m_minSpeedStepInterval;
|
||||
return newStepInterval;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
68
maths.cpp
68
maths.cpp
|
|
@ -1,46 +1,48 @@
|
|||
#include "math.h"
|
||||
#include "turret.h"
|
||||
|
||||
// rollH atan((22.5 - 18.5) / 236) ~0.97°
|
||||
// rollV ~1°
|
||||
|
||||
double degToRad(double deg) { return deg * M_PI / 180.0; }
|
||||
|
||||
double radToDeg(double rad) { return rad * 180.0 / M_PI; }
|
||||
|
||||
void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
|
||||
vec2<long> &step) {
|
||||
step.x = angle.x * stepRatio.x;
|
||||
step.y = angle.y * stepRatio.y;
|
||||
void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
|
||||
// TODO remove magic numbers
|
||||
double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0));
|
||||
double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0));
|
||||
|
||||
stepX = angleX / stepRatioX;
|
||||
stepY = angleY / stepRatioY;
|
||||
}
|
||||
|
||||
void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
|
||||
vec2<double> &angle) {
|
||||
angle.x = step.x / stepRatio.x;
|
||||
angle.y = step.y / stepRatio.y;
|
||||
double rollCorrection(double x, double max, double width) {
|
||||
double correction = max * (1.0 - cos((10.0 * x) / (width * M_PI))) / 2.0;
|
||||
return x - correction;
|
||||
}
|
||||
|
||||
void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
|
||||
vec2<long> zero, vec3<double> position,
|
||||
vec2<long> &step) {
|
||||
// x = -x; // natural axis direction
|
||||
position.y = -position.y; // natural axis direction
|
||||
double dX = position.x + offset.x;
|
||||
double dY = position.y + offset.y;
|
||||
double dZ = position.z + offset.z;
|
||||
void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
|
||||
double x, double y, double z) {
|
||||
// TODO remove magic numbers
|
||||
double distance = 166.0; // turret to screen
|
||||
double offsetY = 21.0; // laser to stand
|
||||
double offsetX = 0.0; // unused
|
||||
|
||||
vec2<double> angle(atan2(dZ, dY) - M_PI / 2, atan2(dZ, dX) - M_PI / 2);
|
||||
x = -x; // natural axis direction
|
||||
double dX = x + offsetX;
|
||||
double dY = rollCorrection(y + offsetY, 0.2, 400.0);
|
||||
double dZ = z + distance;
|
||||
|
||||
radToStep(stepRatio, angle, step);
|
||||
}
|
||||
|
||||
void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
|
||||
vec3<long> current, vec2<long> zero, vec2<long> step,
|
||||
vec3<double> &position) {
|
||||
|
||||
vec2<double> angle;
|
||||
stepToRad(stepRatio, step, angle);
|
||||
|
||||
vec2<double> dXYZ(tan(angle.x) * current.z, tan(angle.y) * current.z);
|
||||
|
||||
position.x = dXYZ.x - offset.x;
|
||||
position.y = -(dXYZ.y - offset.y); // natural axis direction
|
||||
position.z = current.z - offset.z;
|
||||
double roll = atan2(4.0, 236.0);
|
||||
double rollY = dX * tan(roll);
|
||||
double rollX = dY * tan(roll);
|
||||
|
||||
double dXr = dX + rollX;
|
||||
double dYr = dY + rollY;
|
||||
|
||||
double rho = sqrt(pow(dXr, 2.0) + pow(dYr, 2.0) + pow(dZ, 2.0));
|
||||
double angleY = M_PI / 2.0 - acos(dYr / rho);
|
||||
double angleX = atan2(dXr, dZ);
|
||||
|
||||
angleToStep(stepX, stepY, angleX, angleY);
|
||||
}
|
||||
|
|
|
|||
19
maths.h
19
maths.h
|
|
@ -1,18 +1,5 @@
|
|||
#ifndef MATHS_H
|
||||
#define MATHS_H
|
||||
|
||||
#include "turret.h"
|
||||
|
||||
double degToRad(double deg);
|
||||
double radToDeg(double rad);
|
||||
void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
|
||||
vec2<long> &step);
|
||||
void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
|
||||
vec2<double> &angle);
|
||||
void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
|
||||
vec2<long> zero, vec3<double> position, vec2<long> &step);
|
||||
void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
|
||||
vec3<long> current, vec2<long> zero, vec2<long> step,
|
||||
vec3<double> &position);
|
||||
|
||||
#endif
|
||||
void angleToStep(long &stepX, long &stepY, double angleX, double angleY);
|
||||
void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
|
||||
double x, double y, double z);
|
||||
|
|
|
|||
|
|
@ -1,30 +0,0 @@
|
|||
#include "serial_stream.h"
|
||||
|
||||
SerialStream &SerialStream::connect(HardwareSerial &serial) {
|
||||
_serial = &serial;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::disconnect() {
|
||||
_serial = nullptr;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::bind(Turret &turret) {
|
||||
_turret = &turret;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::unbind() {
|
||||
_turret = nullptr;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::loop() {
|
||||
if (_serial == nullptr) return *this;
|
||||
if (_serial->available() == 0) return *this;
|
||||
|
||||
_serial->read();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
#ifndef SERIAL_STREAM_H
|
||||
#define SERIAL_STREAM_H
|
||||
|
||||
#include "HardwareSerial.h"
|
||||
#include "turret.h"
|
||||
|
||||
class SerialStream {
|
||||
public:
|
||||
SerialStream();
|
||||
~SerialStream();
|
||||
|
||||
SerialStream &connect(HardwareSerial &serial);
|
||||
SerialStream &disconnect();
|
||||
|
||||
SerialStream &bind(Turret &turret);
|
||||
SerialStream &unbind();
|
||||
|
||||
SerialStream &loop();
|
||||
|
||||
private:
|
||||
// TEMP to keep this in mind
|
||||
int _internal_buffer;
|
||||
|
||||
HardwareSerial *_serial = nullptr;
|
||||
Turret *_turret = nullptr;
|
||||
};
|
||||
|
||||
#endif
|
||||
458
turret.cpp
458
turret.cpp
|
|
@ -1,432 +1,218 @@
|
|||
#include "turret.h"
|
||||
#include "Arduino.h"
|
||||
#include "math.h"
|
||||
#include "config.h"
|
||||
#include "maths.h"
|
||||
#include <kissStepper.h>
|
||||
|
||||
Turret::Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
|
||||
PinMap pin_map_x, PinMap pin_map_y)
|
||||
: _stepper(kissStepper(static_cast<uint8_t>(pin_map_x.direction),
|
||||
static_cast<uint8_t>(pin_map_x.pulse),
|
||||
static_cast<uint8_t>(pin_map_x.enable)),
|
||||
kissStepper(static_cast<uint8_t>(pin_map_y.direction),
|
||||
static_cast<uint8_t>(pin_map_y.pulse),
|
||||
static_cast<uint8_t>(pin_map_y.enable))) {
|
||||
Turret::Turret(int pin_x_direction, int pin_x_pulse, int pin_x_enable,
|
||||
int pin_x_home, int pin_y_direction, int pin_y_pulse,
|
||||
int pin_y_enable, int pin_y_home, int pin_laser)
|
||||
: _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable),
|
||||
_stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) {
|
||||
|
||||
_pin.x = pin_map_x;
|
||||
_pin.y = pin_map_y;
|
||||
_step_ratio = step_ratio;
|
||||
_offset = offset;
|
||||
_zero_offset = zero_offset;
|
||||
}
|
||||
_pin_x_direction = pin_x_direction;
|
||||
_pin_x_pulse = pin_x_pulse;
|
||||
_pin_x_enable = pin_x_enable;
|
||||
|
||||
Turret::~Turret() {
|
||||
_stepper.x.stop();
|
||||
_stepper.x.disable();
|
||||
_stepper.y.stop();
|
||||
_stepper.y.disable();
|
||||
digitalWrite(_pin.x.laser, LOW);
|
||||
digitalWrite(_pin.y.laser, LOW);
|
||||
_pin_y_direction = pin_y_direction;
|
||||
_pin_y_pulse = pin_y_pulse;
|
||||
_pin_y_enable = pin_y_enable;
|
||||
|
||||
_pin_x_home = pin_x_home;
|
||||
_pin_y_home = pin_y_home;
|
||||
_pin_laser = pin_laser;
|
||||
}
|
||||
|
||||
Turret &Turret::init() {
|
||||
pinMode(_pin.x.laser, OUTPUT);
|
||||
pinMode(_pin.y.laser, OUTPUT);
|
||||
pinMode(_pin_laser, OUTPUT);
|
||||
|
||||
pinMode(_pin.x.direction, OUTPUT);
|
||||
pinMode(_pin.x.pulse, OUTPUT);
|
||||
pinMode(_pin.x.enable, OUTPUT);
|
||||
pinMode(_pin.x.home, INPUT_PULLUP);
|
||||
pinMode(_pin_x_direction, OUTPUT);
|
||||
pinMode(_pin_x_pulse, OUTPUT);
|
||||
pinMode(_pin_x_enable, OUTPUT);
|
||||
pinMode(_pin_x_home, INPUT_PULLUP);
|
||||
|
||||
pinMode(_pin.y.direction, OUTPUT);
|
||||
pinMode(_pin.y.pulse, OUTPUT);
|
||||
pinMode(_pin.y.enable, OUTPUT);
|
||||
pinMode(_pin.y.home, INPUT_PULLUP);
|
||||
pinMode(_pin_y_direction, OUTPUT);
|
||||
pinMode(_pin_y_pulse, OUTPUT);
|
||||
pinMode(_pin_y_enable, OUTPUT);
|
||||
pinMode(_pin_y_home, INPUT_PULLUP);
|
||||
|
||||
_stepper.x.begin();
|
||||
_stepper.y.begin();
|
||||
_stepperX.begin();
|
||||
_stepperY.begin();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::_enqueue_tick(Turret::TickHandler handler) {
|
||||
size_t next = (_tick_queue_tail + 1) % Turret::TICK_QUEUE_LENGTH;
|
||||
if (next == _tick_queue_head) return false; // queue full
|
||||
_tick_queue[_tick_queue_tail] = handler;
|
||||
_tick_queue_tail = next;
|
||||
return true;
|
||||
}
|
||||
Turret &Turret::gotoHome() {
|
||||
long xStop = 0;
|
||||
long yStop = 0;
|
||||
|
||||
bool Turret::_dequeue_tick() {
|
||||
if (_tick_queue_head == _tick_queue_tail) return false; // queue is empty
|
||||
_tick_queue_head = (_tick_queue_head + 1) % Turret::TICK_QUEUE_LENGTH;
|
||||
return true;
|
||||
}
|
||||
_stepperX.prepareMove(-1000000l);
|
||||
_stepperY.prepareMove(-1000000l);
|
||||
|
||||
Turret::TickHandler Turret::_current_tick() {
|
||||
if (_tick_queue_head == _tick_queue_tail) return nullptr;
|
||||
return _tick_queue[_tick_queue_head];
|
||||
}
|
||||
while (true) {
|
||||
_stepperX.move();
|
||||
_stepperY.move();
|
||||
|
||||
Turret &Turret::wait(long ms, bool blocking) {
|
||||
if (blocking) {
|
||||
while(_wait_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_wait_tick;
|
||||
if (xStop == 2 && yStop == 2) {
|
||||
_homeX = _stepperX.getPos();
|
||||
_homeY = _stepperY.getPos();
|
||||
break;
|
||||
}
|
||||
|
||||
if (xStop < 2 && digitalRead(_pin_x_home)) {
|
||||
xStop++;
|
||||
_stepperX.stop();
|
||||
}
|
||||
|
||||
if (yStop < 2 && digitalRead(_pin_y_home)) {
|
||||
yStop++;
|
||||
_stepperY.stop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Turret::_wait_tick() {
|
||||
if (delay + start >= now) return false;
|
||||
return true
|
||||
}
|
||||
|
||||
Turret &Turret::gotoHome(bool blocking) {
|
||||
if (blocking) {
|
||||
while(_goto_home_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_goto_home_tick;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::_goto_home_tick() {
|
||||
_stepper.x.prepareMove(-1000000l);
|
||||
_stepper.y.prepareMove(-1000000l);
|
||||
|
||||
bool xStop = _stepper.x.getState();
|
||||
bool yStop = _stepper.y.getState();
|
||||
Turret &Turret::gotoZero() {
|
||||
// TODO remove magic numbers
|
||||
_stepperX.prepareMove(_homeX + 9300);
|
||||
_stepperY.prepareMove(_homeY + 3500);
|
||||
|
||||
_stepper.x.move();
|
||||
_stepper.y.move();
|
||||
bool xStop = false;
|
||||
bool yStop = false;
|
||||
|
||||
if (xStop && yStop ) {
|
||||
_home.x = _stepper.x.getPos();
|
||||
_home.y = _stepper.y.getPos();
|
||||
return false;
|
||||
while (true) {
|
||||
if (xStop && yStop)
|
||||
break;
|
||||
if (!xStop)
|
||||
xStop = _stepperY.move() == STATE_STOPPED;
|
||||
if (!yStop)
|
||||
yStop = _stepperX.move() == STATE_STOPPED;
|
||||
}
|
||||
|
||||
if (!xStop && digitalRead(_pin.x.home)) {
|
||||
_stepper.x.stop();
|
||||
}
|
||||
_zeroX = _stepperX.getPos();
|
||||
_zeroY = _stepperY.getPos();
|
||||
|
||||
if (!yStop && digitalRead(_pin.y.home)) {
|
||||
_stepper.y.stop();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Turret &Turret::gotoZero(bool blocking) {
|
||||
if (blocking) {
|
||||
while(_goto_zero_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_goto_zero_tick;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::_goto_zero_tick() {
|
||||
_stepper.x.prepareMove(_home.x + _zero_offset.x);
|
||||
_stepper.y.prepareMove(_home.y + _zero_offset.y);
|
||||
|
||||
bool xStop = _stepper.x.getState();
|
||||
bool yStop = _stepper.y.getState();
|
||||
|
||||
if (xStop && yStop) {
|
||||
_zero.x = _stepper.x.getPos();
|
||||
_zero.y = _stepper.y.getPos();
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!xStop)
|
||||
xStop = _stepper.y.move() == STATE_STOPPED;
|
||||
if (!yStop)
|
||||
yStop = _stepper.x.move() == STATE_STOPPED;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Turret &Turret::calibrate(bool blocking) {
|
||||
gotoHome(blocking);
|
||||
gotoZero(blocking);
|
||||
Turret &Turret::calibrate() {
|
||||
gotoHome();
|
||||
gotoZero();
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::moveTo(double x, double y, double z, Unit unit, bool blocking) {
|
||||
vec2<long> step;
|
||||
Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
|
||||
long stepX;
|
||||
long stepY;
|
||||
|
||||
if (unit == Unit::MM) {
|
||||
vec3<double> position(x / 10.0, y / 10.0, z / 10.0);
|
||||
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
||||
cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0,
|
||||
z / 10.0);
|
||||
}
|
||||
|
||||
if (unit == Unit::CM) {
|
||||
vec3<double> position(x, y, z);
|
||||
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
||||
cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x, y, z);
|
||||
}
|
||||
|
||||
if (unit == Unit::M) {
|
||||
vec3<double> position(x * 100.0, y * 100.0, z * 100.0);
|
||||
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
||||
cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0,
|
||||
z * 100.0);
|
||||
}
|
||||
|
||||
if (unit == Unit::STEP) {
|
||||
step.x = x;
|
||||
step.y = y;
|
||||
stepX = x;
|
||||
stepY = y;
|
||||
}
|
||||
|
||||
if (unit == Unit::RAD) {
|
||||
radToStep(_step_ratio, vec2<double>(x, y), step);
|
||||
angleToStep(stepX, stepY, x, y);
|
||||
}
|
||||
|
||||
if (unit == Unit::DEG) {
|
||||
radToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
|
||||
angleToStep(stepX, stepY, degToRad(x), degToRad(y));
|
||||
}
|
||||
|
||||
long maxDeltaStepX = _step_ratio.x * M_PI_2;
|
||||
long maxDeltaStepY = _step_ratio.y * M_PI_2;
|
||||
// TODO min(valueI, -_homeI);
|
||||
_stepperX.prepareMove(_zeroX + stepX);
|
||||
_stepperY.prepareMove(_zeroY + stepY);
|
||||
|
||||
// force move from -90° to 90° avoiding backside,
|
||||
// if move > 180° skip the full turn a start at -90°,
|
||||
// same in the other direction
|
||||
long stepX = constrain(step.x % maxDeltaStepX, -(maxDeltaStepX >> 2),
|
||||
(maxDeltaStepX >> 2));
|
||||
long stepY = constrain(step.y % maxDeltaStepY, -(maxDeltaStepY >> 2),
|
||||
(maxDeltaStepY >> 2));
|
||||
bool xStop = false;
|
||||
bool yStop = false;
|
||||
|
||||
if (blocking) {
|
||||
while(_move_to_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_move_to_tick;
|
||||
while (true) {
|
||||
if (xStop && yStop)
|
||||
break;
|
||||
if (!xStop)
|
||||
xStop = _stepperX.move() == STATE_STOPPED;
|
||||
if (!yStop)
|
||||
yStop = _stepperY.move() == STATE_STOPPED;
|
||||
}
|
||||
// _stepper.x.prepareMove(_zero.x + stepX);
|
||||
// _stepper.y.prepareMove(_zero.y + stepY);
|
||||
|
||||
// bool xStop = false;
|
||||
// bool yStop = false;
|
||||
|
||||
// while (true) {
|
||||
// if (xStop && yStop)
|
||||
// break;
|
||||
// if (!xStop)
|
||||
// xStop = _stepper.x.move() == STATE_STOPPED;
|
||||
// if (!yStop)
|
||||
// yStop = _stepper.y.move() == STATE_STOPPED;
|
||||
// }
|
||||
|
||||
// _current.x = x;
|
||||
// _current.y = y;
|
||||
// _current.z = z;
|
||||
_currentX = x;
|
||||
_currentY = y;
|
||||
_currentZ = z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::_move_to_tick() {
|
||||
stepX;
|
||||
stepY;
|
||||
Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
|
||||
long zeroXStored = _zeroX;
|
||||
long zeroYStored = _zeroY;
|
||||
|
||||
_stepper.x.prepareMove(_zero.x + stepX);
|
||||
_stepper.y.prepareMove(_zero.y + stepY);
|
||||
_zeroX += _stepperX.getPos();
|
||||
_zeroY += _stepperY.getPos();
|
||||
|
||||
bool xStop = _stepper.x.getState() == STATE_STOPPED;
|
||||
bool yStop = _stepper.y.getState() == STATE_STOPPED;
|
||||
moveTo(x, y, z, unit);
|
||||
|
||||
if (xStop && yStop) {
|
||||
_current.x = x;
|
||||
_current.y = y;
|
||||
_current.z = z;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!xStop) xStop = _stepper.x.move() == STATE_STOPPED;
|
||||
if (!yStop) yStop = _stepper.y.move() == STATE_STOPPED;
|
||||
}
|
||||
_zeroX = zeroXStored;
|
||||
_zeroY = zeroYStored;
|
||||
|
||||
Turret &Turret::moveBy(double x, double y, double z, Unit unit, bool blocking) {
|
||||
long zeroXStored = _zero.x;
|
||||
long zeroYStored = _zero.y;
|
||||
|
||||
_zero.x += _stepper.x.getPos();
|
||||
_zero.y += _stepper.y.getPos();
|
||||
|
||||
moveTo(x, y, z, unit, blocking);
|
||||
|
||||
_zero.x = zeroXStored;
|
||||
_zero.y = zeroYStored;
|
||||
|
||||
_current.x = x;
|
||||
_current.y = y;
|
||||
_current.z = z;
|
||||
_currentX = x;
|
||||
_currentY = y;
|
||||
_currentZ = z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::moveToX(double x, Unit unit, bool blocking) {
|
||||
return moveTo(x, _current.y, _current.z, unit, blocking);
|
||||
Turret &Turret::moveToX(double x, Unit unit) {
|
||||
return moveTo(x, _currentY, _currentZ, unit);
|
||||
}
|
||||
|
||||
Turret &Turret::moveToY(double y, Unit unit, bool blocking) {
|
||||
return moveTo(_current.x, y, _current.z, unit, blocking);
|
||||
Turret &Turret::moveToY(double y, Unit unit) {
|
||||
return moveTo(_currentX, y, _currentZ, unit);
|
||||
}
|
||||
|
||||
Turret &Turret::moveToZ(double z, Unit unit, bool blocking) {
|
||||
return moveTo(_current.x, _current.y, z, unit, blocking);
|
||||
Turret &Turret::moveToZ(double z, Unit unit) {
|
||||
return moveTo(_currentX, _currentY, z, unit);
|
||||
}
|
||||
|
||||
Turret &Turret::moveByX(double x, Unit unit, bool blocking) { return moveBy(x, 0, 0, unit, blocking); }
|
||||
Turret &Turret::moveByX(double x, Unit unit) { return moveBy(x, 0, 0, unit); }
|
||||
|
||||
Turret &Turret::moveByY(double y, Unit unit, bool blocking) { return moveTo(0, y, 0, unit, blocking); }
|
||||
Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); }
|
||||
|
||||
Turret &Turret::moveByZ(double z, Unit unit, bool blocking) { return moveTo(0, 0, z, unit, blocking); }
|
||||
|
||||
Turret &Turret::nextTick() {
|
||||
TickHandler tick = _current_tick();
|
||||
if (tick == nullptr) return *this;
|
||||
if (!(this->*tick)()) _dequeue_tick();
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::flushTick() {
|
||||
while (_dequeue_tick());
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::hasTick() {
|
||||
TickHandler tick = _current_tick();
|
||||
return tick != nullptr;
|
||||
}
|
||||
Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); }
|
||||
|
||||
Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
|
||||
if (unit == Unit::MM) {
|
||||
vec3<double> position;
|
||||
vec2<long> xy(_current.x, _current.y);
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
||||
x = position.x / 10;
|
||||
y = position.y / 10;
|
||||
z = position.z / 10;
|
||||
}
|
||||
|
||||
if (unit == Unit::CM) {
|
||||
vec3<double> position;
|
||||
vec2<long> xy(_current.x, _current.y);
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
||||
x = position.x;
|
||||
y = position.y;
|
||||
z = position.z;
|
||||
}
|
||||
|
||||
if (unit == Unit::M) {
|
||||
vec3<double> position;
|
||||
vec2<long> xy(_current.x, _current.y);
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
||||
x = position.x * 100;
|
||||
y = position.y * 100;
|
||||
z = position.z * 100;
|
||||
}
|
||||
|
||||
if (unit == Unit::STEP) {
|
||||
x = _home.x;
|
||||
y = _home.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::RAD) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _home, angle);
|
||||
x = angle.x;
|
||||
y = angle.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::DEG) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _home, angle);
|
||||
x = radToDeg(angle.x);
|
||||
y = radToDeg(angle.y);
|
||||
}
|
||||
|
||||
// TODO implement
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::getHome(double &x, double &y, Unit unit) {
|
||||
if (unit == Unit::MM) {
|
||||
vec3<double> position;
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
||||
x = position.x / 10;
|
||||
y = position.y / 10;
|
||||
}
|
||||
|
||||
if (unit == Unit::CM) {
|
||||
vec3<double> position;
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
||||
x = position.x;
|
||||
y = position.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::M) {
|
||||
vec3<double> position;
|
||||
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
||||
x = position.x * 100;
|
||||
y = position.y * 100;
|
||||
}
|
||||
|
||||
if (unit == Unit::STEP) {
|
||||
x = _home.x;
|
||||
y = _home.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::RAD) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _home, angle);
|
||||
x = angle.x;
|
||||
y = angle.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::DEG) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _home, angle);
|
||||
x = radToDeg(angle.x);
|
||||
y = radToDeg(angle.y);
|
||||
}
|
||||
|
||||
Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) {
|
||||
// TODO implement
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::getZero(double &x, double &y, Unit unit) {
|
||||
if ((unit == Unit::MM) || (unit == Unit::CM) || (unit == Unit::MM)) {
|
||||
x = 0;
|
||||
y = 0;
|
||||
}
|
||||
|
||||
if (unit == Unit::STEP) {
|
||||
x = _zero.x;
|
||||
y = _zero.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::RAD) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _zero, angle);
|
||||
x = angle.x;
|
||||
y = angle.y;
|
||||
}
|
||||
|
||||
if (unit == Unit::DEG) {
|
||||
vec2<double> angle;
|
||||
stepToRad(_step_ratio, _zero, angle);
|
||||
x = radToDeg(angle.x);
|
||||
y = radToDeg(angle.y);
|
||||
}
|
||||
|
||||
Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) {
|
||||
// TODO implement
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::laserOn() {
|
||||
digitalWrite(_pin.x.laser, HIGH);
|
||||
digitalWrite(_pin.y.laser, HIGH);
|
||||
digitalWrite(_pin_laser, HIGH);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::laserOff() {
|
||||
digitalWrite(_pin.x.laser, LOW);
|
||||
digitalWrite(_pin.y.laser, LOW);
|
||||
digitalWrite(_pin_laser, LOW);
|
||||
return *this;
|
||||
}
|
||||
|
|
|
|||
109
turret.h
109
turret.h
|
|
@ -3,102 +3,63 @@
|
|||
|
||||
#include <kissStepper.h>
|
||||
|
||||
template <typename T> struct vec3 {
|
||||
T x;
|
||||
T y;
|
||||
T z;
|
||||
|
||||
vec3() : x(T()), y(T()), z(T()) {}
|
||||
vec3(T _x, T _y, T _z) : x(_x), y(_y), z(_z) {}
|
||||
};
|
||||
|
||||
template <typename T> struct vec2 {
|
||||
T x;
|
||||
T y;
|
||||
|
||||
vec2() : x(T()), y(T()) {}
|
||||
vec2(T _x, T _y) : x(_x), y(_y) {}
|
||||
};
|
||||
|
||||
class Turret {
|
||||
public:
|
||||
enum Unit { MM, CM, M, RAD, DEG, STEP };
|
||||
|
||||
using StepRatio = vec2<double>;
|
||||
using Offset = vec3<double>;
|
||||
// x -> turret to screen
|
||||
// y -> laser to stand
|
||||
// z -> unused
|
||||
|
||||
using ZeroOffset = vec2<long>;
|
||||
|
||||
struct PinMap {
|
||||
int home;
|
||||
int direction;
|
||||
int pulse;
|
||||
int enable;
|
||||
int laser;
|
||||
};
|
||||
|
||||
Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
|
||||
PinMap pin_map_x, PinMap pin_map_y);
|
||||
~Turret();
|
||||
Turret(int pin_x_direction, int pin_x_pulse, int pin_x_enable, int pin_x_home,
|
||||
int pin_y_direction, int pin_y_pulse, int pin_y_enable, int pin_y_home,
|
||||
int pin_laser);
|
||||
|
||||
Turret &init();
|
||||
Turret &gotoHome(bool blocking = false);
|
||||
Turret &gotoZero(bool blocking = false);
|
||||
Turret &gotoHome();
|
||||
Turret &gotoZero();
|
||||
|
||||
Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM);
|
||||
|
||||
Turret &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveToX(double x, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveToY(double y, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveToZ(double z, Unit unit = Turret::Unit::CM);
|
||||
|
||||
Turret &moveByX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveByZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveByX(double x, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveByY(double y, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
|
||||
|
||||
Turret &getPosition(double &x, double &y, double &z, Unit unit);
|
||||
Turret &getHome(double &x, double &y, Unit unit);
|
||||
Turret &getZero(double &x, double &y, Unit unit);
|
||||
Turret &getHome(double &x, double &y, double &z, Unit unit);
|
||||
Turret &getZero(double &x, double &y, double &z, Unit unit);
|
||||
|
||||
Turret &calibrate(bool blocking = false);
|
||||
Turret &calibrate();
|
||||
|
||||
Turret &laserOn();
|
||||
Turret &laserOff();
|
||||
|
||||
Turret &nextTick();
|
||||
Turret &flushTick();
|
||||
bool hasTick();
|
||||
|
||||
private:
|
||||
vec2<kissStepper> _stepper;
|
||||
vec2<long> _home;
|
||||
vec2<long> _zero;
|
||||
vec3<long> _current;
|
||||
vec2<PinMap> _pin;
|
||||
StepRatio _step_ratio;
|
||||
Offset _offset;
|
||||
ZeroOffset _zero_offset;
|
||||
kissStepper _stepperX;
|
||||
kissStepper _stepperY;
|
||||
|
||||
// tick queue related
|
||||
static constexpr size_t TICK_QUEUE_LENGTH = 32;
|
||||
using TickHandler = bool (Turret::*)();
|
||||
int _homeX;
|
||||
int _homeY;
|
||||
|
||||
TickHandler _tick_queue[TICK_QUEUE_LENGTH];
|
||||
size_t _tick_queue_head = 0;
|
||||
size_t _tick_queue_tail = 0;
|
||||
int _zeroX;
|
||||
int _zeroY;
|
||||
|
||||
bool _enqueue_tick(TickHandler fn);
|
||||
bool _dequeue_tick();
|
||||
TickHandler _current_tick();
|
||||
int _currentX;
|
||||
int _currentY;
|
||||
int _currentZ;
|
||||
|
||||
bool (Turret::*_next_tick)() = nullptr;
|
||||
int _pin_x_direction;
|
||||
int _pin_x_pulse;
|
||||
int _pin_x_enable;
|
||||
|
||||
// tick handlers
|
||||
bool _goto_zero_tick();
|
||||
bool _goto_home_tick();
|
||||
int _pin_y_direction;
|
||||
int _pin_y_pulse;
|
||||
int _pin_y_enable;
|
||||
|
||||
int _pin_x_home;
|
||||
int _pin_y_home;
|
||||
int _pin_laser;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
103
turret_debug.ino
103
turret_debug.ino
|
|
@ -1,34 +1,17 @@
|
|||
#include "config.h"
|
||||
#include "turret.h"
|
||||
|
||||
Turret::PinMap pinX = {
|
||||
.home = PIN_X_HOME,
|
||||
.direction = PIN_X_DIRECTION,
|
||||
.pulse = PIN_X_PULSE,
|
||||
.enable = PIN_X_ENABLE,
|
||||
.laser = PIN_LASER,
|
||||
};
|
||||
|
||||
Turret::PinMap pinY = {
|
||||
.home = PIN_Y_HOME,
|
||||
.direction = PIN_Y_DIRECTION,
|
||||
.pulse = PIN_Y_PULSE,
|
||||
.enable = PIN_Y_ENABLE,
|
||||
.laser = PIN_LASER,
|
||||
};
|
||||
|
||||
Turret::StepRatio stepRatio(STEP_RATIO_X, STEP_RATIO_Y);
|
||||
Turret::Offset offset(OFFSET_X, OFFSET_Y, OFFSET_Z);
|
||||
Turret::ZeroOffset zeroOffset(ZERO_OFFSET_X, ZERO_OFFSET_Y);
|
||||
|
||||
Turret turret(stepRatio, offset, zeroOffset, pinX, pinY);
|
||||
Turret turret(PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME,
|
||||
PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME,
|
||||
PIN_LASER);
|
||||
|
||||
void setup() {
|
||||
turret.init();
|
||||
|
||||
Serial.begin(BAUDRATE);
|
||||
|
||||
turret.laserOn().calibrate().moveTo(0, 0, 0);
|
||||
turret.laserOn().calibrate();
|
||||
turret.moveTo(0, 0, 0);
|
||||
delay(5000);
|
||||
|
||||
// test pointage
|
||||
|
|
@ -37,22 +20,74 @@ void setup() {
|
|||
int panelHCount = 3;
|
||||
int panelVCount = 2;
|
||||
|
||||
double zeroOffsetV = -21;
|
||||
double zeroOffsetH = -163;
|
||||
// double zeroOffsetH = -147;
|
||||
// double zeroOffsetV = -31.5;
|
||||
double zeroOffsetV = -20.8;
|
||||
// double zeroOffsetH = -184.5;
|
||||
double zeroOffsetH = -195.5;
|
||||
|
||||
// Test constant step h,v
|
||||
for (double step = 0; step < 160; step += 10) {
|
||||
Serial.print("[step]: ");
|
||||
Serial.println(step);
|
||||
turret.moveTo(step, 0, 0);
|
||||
Serial.println("Start demo_0");
|
||||
for (int i = 0; i < 20; i++) {
|
||||
Serial.print("> ");
|
||||
Serial.println(i);
|
||||
for (double step = -198.5; step < 176.0; step += 10) {
|
||||
Serial.print("[step]: ");
|
||||
Serial.println(step);
|
||||
turret.moveTo(step, 87, 0);
|
||||
// turret.moveBy(20.0, 0.0, 0.0);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
for (double step = 0; step < 200.0; step += 10) {
|
||||
Serial.print("[step]: ");
|
||||
Serial.println(step);
|
||||
turret.moveTo(0, step, 0);
|
||||
// turret.moveBy(20.0, 0.0, 0.0);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
delay(5000);
|
||||
turret.moveTo(0, 0, 0);
|
||||
delay(5000);
|
||||
}
|
||||
for (double step = 0; step > -160; step -= 10) {
|
||||
Serial.print("[step]: ");
|
||||
Serial.println(step);
|
||||
turret.moveTo(step, 0, 0);
|
||||
delay(5000);
|
||||
|
||||
// Test rollX
|
||||
turret.moveTo(0 * panelWidth + zeroOffsetH, 0, 0);
|
||||
delay(5000);
|
||||
turret.moveTo((panelHCount * panelWidth) / 2 + zeroOffsetH, 0, 0);
|
||||
delay(5000);
|
||||
turret.moveTo(panelHCount * panelWidth + zeroOffsetH, 0, 0);
|
||||
delay(5000);
|
||||
turret.gotoZero();
|
||||
|
||||
// Test rollY
|
||||
turret.moveTo(0, 0 * panelHeight + zeroOffsetV, 0);
|
||||
delay(5000);
|
||||
turret.moveTo(0, (panelVCount * panelHeight) / 2 + zeroOffsetV, 0);
|
||||
delay(5000);
|
||||
turret.moveTo(0, panelVCount * panelHeight + zeroOffsetV, 0);
|
||||
delay(5000);
|
||||
turret.gotoZero();
|
||||
|
||||
// Test align to panels
|
||||
for (double panelH = 0; panelH < panelHCount + 1; panelH += 0.5) {
|
||||
for (double panelV = 0; panelV < panelVCount + 1; panelV += 0.5) {
|
||||
double x = panelH * panelWidth + zeroOffsetH;
|
||||
double y = panelV * panelHeight + zeroOffsetV;
|
||||
double z = 0;
|
||||
|
||||
Serial.print("Goto (x, y, z) => (");
|
||||
Serial.print(x);
|
||||
Serial.print(", ");
|
||||
Serial.print(y);
|
||||
Serial.print(", ");
|
||||
Serial.print(z);
|
||||
Serial.println(")");
|
||||
|
||||
turret.moveTo(x, y, z);
|
||||
|
||||
delay(2000);
|
||||
}
|
||||
}
|
||||
|
||||
turret.gotoZero();
|
||||
|
|
|
|||
Loading…
Reference in a new issue