Initial commit
First and incomplete version of the refactor, stage module works, including aborting moves.
This commit is contained in:
Filip Ayazi
commit
2f78522064
16 changed files with 1264 additions and 0 deletions
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,5 @@
|
|||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
7
.vscode/extensions.json
vendored
Normal file
7
.vscode/extensions.json
vendored
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
{
|
||||
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||
// for the documentation about the extensions.json format
|
||||
"recommendations": [
|
||||
"platformio.platformio-ide"
|
||||
]
|
||||
}
|
||||
30
README.md
Normal file
30
README.md
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
# Firmware code for the sangaboard
|
||||
|
||||
*This firmware is Work in Progress and has not reached feature parity with the original yet (see below for more details)*
|
||||
|
||||
Firmware for the control board of OpenFlexure Microscope. This is a refactor of the (original arduino code](https://gitlab.com/bath_open_instrumentation_group/sangaboard/-/blob/master/arduino_code/README.md) with some improvements to the infrastructure and features.
|
||||
|
||||
## Notable changes
|
||||
This version of the firmware uses PlatformIO rather than Arduino IDE.
|
||||
|
||||
The code is split into the main part which mostly handles command parsing and modules which execute actual commands, allowing the firmware to be easily extensible with new features (add a module based on one of the existing ones and add a line to setup() to register it's commands and loop function).
|
||||
|
||||
Motor moves now do not block command processing and a new command `stop` was added which aborts any move in progress.
|
||||
|
||||
## Hardware
|
||||
|
||||
Currently works on Arduino Nano + Sangaboard v0.2. Other platforms might work with appropriate `platform.ini` and `config.h` changes and will be supported in due course.
|
||||
|
||||
## TODO list
|
||||
### Original firmware feature parity
|
||||
- [] Implement Endstops module
|
||||
- [] Implement Light sensor modules
|
||||
- [] Handle Sangaboardv3
|
||||
- [] Ensure responses to commands match the original version
|
||||
|
||||
### Improvements and fixes
|
||||
- [] Add a CI script
|
||||
- [] Add tests (argument parsing)
|
||||
- [] Test automatic building and upload
|
||||
- [] Support other platforms
|
||||
- [] Clean up some of the messier bits
|
||||
39
include/README
Normal file
39
include/README
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
|
||||
This directory is intended for project header files.
|
||||
|
||||
A header file is a file containing C declarations and macro definitions
|
||||
to be shared between several project source files. You request the use of a
|
||||
header file in your project source file (C, C++, etc) located in `src` folder
|
||||
by including it, with the C preprocessing directive `#include'.
|
||||
|
||||
```src/main.c
|
||||
|
||||
#include "header.h"
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Including a header file produces the same results as copying the header file
|
||||
into each source file that needs it. Such copying would be time-consuming
|
||||
and error-prone. With a header file, the related declarations appear
|
||||
in only one place. If they need to be changed, they can be changed in one
|
||||
place, and programs that include the header file will automatically use the
|
||||
new version when next recompiled. The header file eliminates the labor of
|
||||
finding and changing all the copies as well as the risk that a failure to
|
||||
find one copy will result in inconsistencies within a program.
|
||||
|
||||
In C, the usual convention is to give header files names that end with `.h'.
|
||||
It is most portable to use only letters, digits, dashes, and underscores in
|
||||
header file names, and at most one dot.
|
||||
|
||||
Read more about using header files in official GCC documentation:
|
||||
|
||||
* Include Syntax
|
||||
* Include Operation
|
||||
* Once-Only Headers
|
||||
* Computed Includes
|
||||
|
||||
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||
46
lib/README
Normal file
46
lib/README
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into executable file.
|
||||
|
||||
The source code of each library should be placed in a an own separate directory
|
||||
("lib/your_library_name/[here are source files]").
|
||||
|
||||
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
and a contents of `src/main.c`:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
15
platformio.ini
Normal file
15
platformio.ini
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
; PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:nanoatmega328new]
|
||||
platform = atmelavr
|
||||
board = nanoatmega328new
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
27
src/config.h
Normal file
27
src/config.h
Normal file
|
|
@ -0,0 +1,27 @@
|
|||
#ifndef CONFIGURED
|
||||
//general settings
|
||||
#define MAX_COMMANDS 50
|
||||
#define MAX_MODULES 10
|
||||
#define VERSION_STRING "Sangaboard Firmware v0.6"
|
||||
#define DEBUG_ON
|
||||
|
||||
//module choice
|
||||
#define HELP
|
||||
#define STAGE
|
||||
|
||||
//module configs
|
||||
#ifdef STAGE
|
||||
//TODO: Fix this for platformio
|
||||
#ifdef ARDUINO_AVR_LEONARDO
|
||||
#define SANGABOARDv3
|
||||
#define BOARD_STRING "Sangaboard v0.3"
|
||||
#elif ARDUINO_AVR_SANGABOARD
|
||||
#define SANGABOARDv3
|
||||
#define BOARD_STRING "Sangaboard v0.3"
|
||||
#else
|
||||
#define SANGABOARDv2
|
||||
#define BOARD_STRING "Sangaboard v0.2"
|
||||
#endif
|
||||
#endif
|
||||
#define CONFIGURED
|
||||
#endif
|
||||
131
src/main.cpp
Normal file
131
src/main.cpp
Normal file
|
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
* Sangaboard firmware
|
||||
*
|
||||
* Refactored from bath_open_instrumentation_group/sangaboard/arduino_code
|
||||
*
|
||||
* This firmware was written by
|
||||
* Richard Bowman
|
||||
* Julian Stirling
|
||||
* Boyko Vodenicharski
|
||||
* Filip Ayazi
|
||||
*
|
||||
* Much of the code is based on older code written by
|
||||
* James Sharkey and Fergus Riche
|
||||
*
|
||||
* Released under GPL v3, 2021
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <stdint.h>
|
||||
#include "config.h"
|
||||
#include "main.h"
|
||||
|
||||
#ifdef HELP
|
||||
#include "modules/help/help.h"
|
||||
#endif
|
||||
|
||||
#ifdef STAGE
|
||||
#include "modules/stage/stage.h"
|
||||
#endif
|
||||
|
||||
Command registered_commands[MAX_COMMANDS];
|
||||
void (*registered_loop_functions[MAX_MODULES])(void);
|
||||
|
||||
uint16_t registered_commands_count = 0;
|
||||
uint8_t registered_loop_fn_count = 0;
|
||||
|
||||
const Command core_commands[] = {
|
||||
{"version", get_version},
|
||||
END_COMMAND};
|
||||
|
||||
void register_module(const Command commands[], void (*loop_fn)(void))
|
||||
{
|
||||
for (int i = 0; CHECK_END_COMMAND(commands[i]); i++)
|
||||
registered_commands[registered_commands_count++] = commands[i];
|
||||
|
||||
if (loop_fn)
|
||||
registered_loop_functions[registered_loop_fn_count++] = loop_fn;
|
||||
}
|
||||
|
||||
void handle_command(String received_command)
|
||||
{
|
||||
for (int i = 0; CHECK_END_COMMAND(registered_commands[i]); i++)
|
||||
{
|
||||
if (received_command.startsWith(registered_commands[i].command))
|
||||
return registered_commands[i].run(received_command.substring(registered_commands[i].command.length()));
|
||||
}
|
||||
|
||||
#ifdef HELP
|
||||
Serial.println(F("Type 'help' for a list of commands."));
|
||||
#else
|
||||
Serial.println(F("Invalid command"));
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t parse_arguments(String arguments[], String command, uint8_t max_args)
|
||||
{
|
||||
uint8_t from = 0;
|
||||
uint8_t parsed = 0;
|
||||
while (from < command.length() && parsed < max_args)
|
||||
{
|
||||
int space = command.indexOf(" ", from + 1);
|
||||
if (space == -1)
|
||||
space = command.length();
|
||||
arguments[parsed] = command.substring(from, space);
|
||||
arguments[parsed++].trim();
|
||||
|
||||
from = space;
|
||||
}
|
||||
|
||||
return parsed;
|
||||
}
|
||||
|
||||
void get_version(String)
|
||||
{
|
||||
Serial.println(F(VER_STRING));
|
||||
return;
|
||||
}
|
||||
|
||||
#ifndef UNIT_TEST
|
||||
void setup()
|
||||
{
|
||||
// initialise serial port
|
||||
Serial.begin(115200);
|
||||
while (!Serial)
|
||||
delay(1);
|
||||
|
||||
register_module(core_commands, NULL);
|
||||
|
||||
#ifdef HELP
|
||||
// D(help_commands_t[0].command);
|
||||
help_setup();
|
||||
#endif
|
||||
|
||||
//register modules
|
||||
#ifdef STAGE
|
||||
stage_setup();
|
||||
#endif
|
||||
|
||||
#ifdef LIGHT_SENSOR
|
||||
light_sensor_setup(); //TODO
|
||||
#endif
|
||||
|
||||
#ifdef ENDSTOPS
|
||||
light_sensor_setup(); //TODO
|
||||
#endif
|
||||
|
||||
registered_commands[registered_commands_count] = END_COMMAND;
|
||||
Serial.println(F(VERSION_STRING));
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
if (Serial.available())
|
||||
{
|
||||
handle_command(Serial.readStringUntil('\n'));
|
||||
}
|
||||
|
||||
for (int i = 0; i < registered_loop_fn_count; i++)
|
||||
registered_loop_functions[i]();
|
||||
}
|
||||
#endif
|
||||
27
src/main.h
Normal file
27
src/main.h
Normal file
|
|
@ -0,0 +1,27 @@
|
|||
#ifndef MAIN_H
|
||||
#include "config.h"
|
||||
#include <Arduino.h>
|
||||
#ifdef DEBUG_ON
|
||||
#define D(x) Serial.println(x)
|
||||
#else
|
||||
#define D(x)
|
||||
#endif
|
||||
|
||||
#define END_COMMAND \
|
||||
{ \
|
||||
"", NULL \
|
||||
}
|
||||
#define CHECK_END_COMMAND(C) C.command.length() > 0
|
||||
|
||||
struct Command
|
||||
{
|
||||
String command;
|
||||
void (*run)(String);
|
||||
};
|
||||
|
||||
uint8_t parse_arguments(String[], String, uint8_t);
|
||||
extern void register_module(const Command commands[], void (*loop_fn)(void));
|
||||
|
||||
void get_version(String);
|
||||
#define MAIN_H
|
||||
#endif
|
||||
71
src/modules/help/help.cpp
Normal file
71
src/modules/help/help.cpp
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#include "help.h"
|
||||
|
||||
void help_setup()
|
||||
{
|
||||
register_module(help_commands, NULL);
|
||||
}
|
||||
|
||||
void help(String command)
|
||||
{
|
||||
Serial.println("");
|
||||
Serial.print("Board: ");
|
||||
Serial.println(F(BOARD_STRING));
|
||||
|
||||
#ifdef LIGHT_SENSOR
|
||||
#if defined ADAFRUIT_TSL2591
|
||||
Serial.println(F("Compiled with Adafruit TSL2591 support"));
|
||||
#elif defined ADAFRUIT_ADS1115
|
||||
Serial.println(F("Compiled with Adafruit ADS1115 support"));
|
||||
#endif
|
||||
#endif
|
||||
#ifdef ENDSTOPS
|
||||
#ifdef ENDSTOPS_MIN
|
||||
Serial.println(F("Compiled with min endstops support"));
|
||||
#endif
|
||||
#ifdef ENDSTOPS_MAX
|
||||
Serial.println(F("Compiled with max endstops support"));
|
||||
#endif
|
||||
#endif
|
||||
|
||||
Serial.println("");
|
||||
Serial.println(F("Commands (terminated by a newline character):"));
|
||||
#ifdef STAGE
|
||||
Serial.println(F("mrx <d> - relative move in x"));
|
||||
Serial.println(F("mry <d> - relative move in y"));
|
||||
Serial.println(F("mrz <d> - relative move in z"));
|
||||
Serial.println(F("mr <d> <d> <d> - relative move in all 3 axes"));
|
||||
Serial.println(F("release - de-energise all motors"));
|
||||
Serial.println(F("p? - print position (3 space-separated integers"));
|
||||
Serial.println(F("ramp_time <d> - set the time taken to accelerate/decelerate in us"));
|
||||
Serial.println(F("min_step_delay <d> - set the minimum time between steps in us."));
|
||||
Serial.println(F("dt <d> - set the minimum time between steps in us."));
|
||||
Serial.println(F("ramp_time? - get the time taken to accelerate/decelerate in us"));
|
||||
Serial.println(F("min_step_delay? - get the minimum time between steps in us."));
|
||||
Serial.println(F("zero - set the current position to zero."));
|
||||
Serial.println(F("stop - stop a move in progress."));
|
||||
|
||||
#endif
|
||||
#ifdef LIGHT_SENSOR
|
||||
Serial.println(F("light_sensor_gain <d> - set the gain of the light sensor"));
|
||||
Serial.println(F("light_sensor_gain? - get the gain of the light sensor"));
|
||||
Serial.println(F("light_sensor_gain_values? - get the allowable gain values of the light sensor"));
|
||||
Serial.println(F("light_sensor_integration_time? - get the integration time in milliseconds"));
|
||||
Serial.println(F("light_sensor_intensity? - read the current value from the full spectrum diode"));
|
||||
#endif //LIGHT_SENSOR
|
||||
|
||||
#if defined(ENDSTOPS_MIN)||defined(ENDSTOPS_MAX)
|
||||
Serial.println(F("endstops? - get triggered endstops in (1,0,-1) format for max, none, min"));
|
||||
Serial.println(F("home_min <axes?> - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
|
||||
Serial.println(F("home_max <axes?> - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
|
||||
Serial.println(F("max_p? - return positions of max endstops"));
|
||||
Serial.println(F("max <d> <d> <d> - set maximum positions"));
|
||||
#endif
|
||||
|
||||
//Serial.println(F("test_mode <s> - set test_mode <on> <off>"));
|
||||
Serial.println(F("version - get firmware version string"));
|
||||
Serial.println("");
|
||||
Serial.println("Input Key:");
|
||||
Serial.println(F("<d> - a decimal integer."));
|
||||
Serial.println("");
|
||||
Serial.println("--END--");
|
||||
}
|
||||
12
src/modules/help/help.h
Normal file
12
src/modules/help/help.h
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
#ifndef HELP_H
|
||||
#include <Arduino.h>
|
||||
#include "main.h"
|
||||
|
||||
void help(String);
|
||||
void help_setup();
|
||||
const struct Command help_commands[] = {
|
||||
{"help", help},
|
||||
END_COMMAND
|
||||
};
|
||||
#define HELP_H
|
||||
#endif
|
||||
415
src/modules/stage/StepperF_alt.cpp
Normal file
415
src/modules/stage/StepperF_alt.cpp
Normal file
|
|
@ -0,0 +1,415 @@
|
|||
/*
|
||||
*§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
|
||||
*Fergus Riche's Updated Stepper Library
|
||||
*This library can drive four wire unipolar motors in half stepping mode.
|
||||
*It was written to drive 28BYJ-48 – 5V Stepper Motors.
|
||||
*
|
||||
*§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
|
||||
*
|
||||
*
|
||||
* Stepper.cpp - Stepper library for Wiring/Arduino - Version 1.1.0
|
||||
*
|
||||
* Original library (0.1) by Tom Igoe.
|
||||
* Two-wire modifications (0.2) by Sebastian Gassner
|
||||
* Combination version (0.3) by Tom Igoe and David Mellis
|
||||
* Bug fix for four-wire (0.4) by Tom Igoe, bug fix from Noah Shibley
|
||||
* High-speed stepping mod by Eugene Kozlenko
|
||||
* Timer rollover fix by Eugene Kozlenko
|
||||
* Five phase five wire (1.1.0) by Ryan Orendorff
|
||||
*
|
||||
* 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 2.1 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
|
||||
*
|
||||
*
|
||||
* Drives a unipolar, bipolar, or five phase stepper motor.
|
||||
*
|
||||
* When wiring multiple stepper motors to a microcontroller, you quickly run
|
||||
* out of output pins, with each motor requiring 4 connections.
|
||||
*
|
||||
* By making use of the fact that at any time two of the four motor coils are
|
||||
* the inverse of the other two, the number of control connections can be
|
||||
* reduced from 4 to 2 for the unipolar and bipolar motors.
|
||||
*
|
||||
* A slightly modified circuit around a Darlington transistor array or an
|
||||
* L293 H-bridge connects to only 2 microcontroler pins, inverts the signals
|
||||
* received, and delivers the 4 (2 plus 2 inverted ones) output signals
|
||||
* required for driving a stepper motor. Similarly the Arduino motor shields
|
||||
* 2 direction pins may be used.
|
||||
*
|
||||
* The sequence of control signals for 5 phase, 5 control wires is as follows:
|
||||
*
|
||||
* Step C0 C1 C2 C3 C4
|
||||
* 1 0 1 1 0 1
|
||||
* 2 0 1 0 0 1
|
||||
* 3 0 1 0 1 1
|
||||
* 4 0 1 0 1 0
|
||||
* 5 1 1 0 1 0
|
||||
* 6 1 0 0 1 0
|
||||
* 7 1 0 1 1 0
|
||||
* 8 1 0 1 0 0
|
||||
* 9 1 0 1 0 1
|
||||
* 10 0 0 1 0 1
|
||||
*
|
||||
* The sequence of control signals for 4 control wires is as follows:
|
||||
*
|
||||
* Step C0 C1 C2 C3
|
||||
* 1 1 0 1 0
|
||||
* 2 0 1 1 0
|
||||
* 3 0 1 0 1
|
||||
* 4 1 0 0 1
|
||||
*
|
||||
* The sequence of controls signals for 2 control wires is as follows
|
||||
* (columns C1 and C2 from above):
|
||||
*
|
||||
* Step C0 C1
|
||||
* 1 0 1
|
||||
* 2 1 1
|
||||
* 3 1 0
|
||||
* 4 0 0
|
||||
*
|
||||
* The circuits can be found at
|
||||
*
|
||||
* http://www.arduino.cc/en/Tutorial/Stepper
|
||||
*/
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "StepperF_alt.h"
|
||||
#include <limits.h>
|
||||
|
||||
///*
|
||||
// * two-wire constructor.
|
||||
// * Sets which wires should control the motor.
|
||||
// */
|
||||
//Stepper::Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2)
|
||||
//{
|
||||
// this->step_number = 0; // which step the motor is on
|
||||
// this->direction = 0; // motor direction
|
||||
// this->last_step_time = 0; // time stamp in us of the last step taken
|
||||
// this->number_of_steps = number_of_steps; // total number of steps for this motor
|
||||
//
|
||||
// // Arduino pins for the motor control connection:
|
||||
// this->motor_pin_1 = motor_pin_1;
|
||||
// this->motor_pin_2 = motor_pin_2;
|
||||
//
|
||||
// // setup the pins on the microcontroller:
|
||||
// pinMode(this->motor_pin_1, OUTPUT);
|
||||
// pinMode(this->motor_pin_2, OUTPUT);
|
||||
//
|
||||
// // When there are only 2 pins, set the others to 0:
|
||||
// this->motor_pin_3 = 0;
|
||||
// this->motor_pin_4 = 0;
|
||||
// this->motor_pin_5 = 0;
|
||||
//
|
||||
// // pin_count is used by the stepMotor() method:
|
||||
// this->pin_count = 2;
|
||||
//}
|
||||
|
||||
|
||||
/*
|
||||
* constructor for four-pin version
|
||||
* Sets which wires should control the motor.
|
||||
*/
|
||||
Stepper::Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2,
|
||||
int motor_pin_3, int motor_pin_4)
|
||||
{
|
||||
this->step_number = 0; // which step the motor is on
|
||||
this->direction = 0; // motor direction
|
||||
this->last_step_time = 0; // time stamp in us of the last step taken
|
||||
this->number_of_steps = number_of_steps; // total number of steps for this motor
|
||||
|
||||
// Arduino pins for the motor control connection:
|
||||
this->motor_pin_1 = motor_pin_1;
|
||||
this->motor_pin_2 = motor_pin_2;
|
||||
this->motor_pin_3 = motor_pin_3;
|
||||
this->motor_pin_4 = motor_pin_4;
|
||||
|
||||
// setup the pins on the microcontroller:
|
||||
pinMode(this->motor_pin_1, OUTPUT);
|
||||
pinMode(this->motor_pin_2, OUTPUT);
|
||||
pinMode(this->motor_pin_3, OUTPUT);
|
||||
pinMode(this->motor_pin_4, OUTPUT);
|
||||
|
||||
// When there are 4 pins, set the others to 0:
|
||||
this->motor_pin_5 = 0;
|
||||
|
||||
// pin_count is used by the stepMotor() method:
|
||||
this->pin_count = 4;
|
||||
}
|
||||
|
||||
///*
|
||||
// * constructor for five phase motor with five wires
|
||||
// * Sets which wires should control the motor.
|
||||
// */
|
||||
//Stepper::Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2,
|
||||
// int motor_pin_3, int motor_pin_4,
|
||||
// int motor_pin_5)
|
||||
//{
|
||||
// this->step_number = 0; // which step the motor is on
|
||||
// this->direction = 0; // motor direction
|
||||
// this->last_step_time = 0; // time stamp in us of the last step taken
|
||||
// this->number_of_steps = number_of_steps; // total number of steps for this motor
|
||||
//
|
||||
// // Arduino pins for the motor control connection:
|
||||
// this->motor_pin_1 = motor_pin_1;
|
||||
// this->motor_pin_2 = motor_pin_2;
|
||||
// this->motor_pin_3 = motor_pin_3;
|
||||
// this->motor_pin_4 = motor_pin_4;
|
||||
// this->motor_pin_5 = motor_pin_5;
|
||||
//
|
||||
// // setup the pins on the microcontroller:
|
||||
// pinMode(this->motor_pin_1, OUTPUT);
|
||||
// pinMode(this->motor_pin_2, OUTPUT);
|
||||
// pinMode(this->motor_pin_3, OUTPUT);
|
||||
// pinMode(this->motor_pin_4, OUTPUT);
|
||||
// pinMode(this->motor_pin_5, OUTPUT);
|
||||
//
|
||||
// // pin_count is used by the stepMotor() method:
|
||||
// this->pin_count = 5;
|
||||
//}
|
||||
//
|
||||
/*
|
||||
* Sets the speed in revs per minute
|
||||
*/
|
||||
void Stepper::setSpeed(long whatSpeed)
|
||||
{
|
||||
this->step_delay = 60L * 1000L * 1000L / this->number_of_steps / whatSpeed;
|
||||
}
|
||||
|
||||
/*
|
||||
* Moves the motor steps_to_move steps. If the number is negative,
|
||||
* the motor moves in the reverse direction.
|
||||
*/
|
||||
void Stepper::step(int steps_to_move)
|
||||
{
|
||||
int steps_left = abs(steps_to_move); // how many steps to take
|
||||
|
||||
// determine direction based on whether steps_to_mode is + or -:
|
||||
if (steps_to_move > 0) { this->direction = 1; }
|
||||
if (steps_to_move < 0) { this->direction = 0; }
|
||||
|
||||
|
||||
// decrement the number of steps, moving one step each time:
|
||||
while (steps_left > 0)
|
||||
{
|
||||
unsigned long now = micros();
|
||||
unsigned long time_since_last;
|
||||
|
||||
//micros() overflows after ~70 min runtime
|
||||
if(now < this->last_step_time)
|
||||
time_since_last=now+(ULONG_MAX-this->last_step_time);
|
||||
else
|
||||
time_since_last=now-this->last_step_time;
|
||||
|
||||
// move only if the appropriate delay has passed:
|
||||
if (time_since_last >= this->step_delay)
|
||||
{
|
||||
// get the timeStamp of when you stepped:
|
||||
this->last_step_time = now;
|
||||
// increment or decrement the step number,
|
||||
// depending on direction:
|
||||
if (this->direction == 1)
|
||||
{
|
||||
this->step_number++;
|
||||
if (this->step_number == this->number_of_steps) {
|
||||
this->step_number = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (this->step_number == 0) {
|
||||
this->step_number = this->number_of_steps;
|
||||
}
|
||||
this->step_number--;
|
||||
}
|
||||
// decrement the steps left:
|
||||
steps_left--;
|
||||
// step the motor to step number 0, 1, ..., {3 or 10}
|
||||
// if (this->pin_count == 5)
|
||||
// stepMotor(this->step_number % 10);
|
||||
// else
|
||||
stepMotor(this->step_number % 8);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Moves the motor forward or backwards.
|
||||
*/
|
||||
void Stepper::stepMotor(int thisStep)
|
||||
{
|
||||
// if (this->pin_count == 2) {
|
||||
// switch (thisStep) {
|
||||
// case 0: // 01
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// break;
|
||||
// case 1: // 11
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// break;
|
||||
// case 2: // 10
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// break;
|
||||
// case 3: // 00
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
// if (this->pin_count == 4) {
|
||||
switch (thisStep) {
|
||||
case 0: // 1000
|
||||
digitalWrite(motor_pin_1, HIGH);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
case 1: // 1100
|
||||
digitalWrite(motor_pin_1, HIGH);
|
||||
digitalWrite(motor_pin_2, HIGH);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
case 2: //0100
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, HIGH);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
case 3: //0110
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, HIGH);
|
||||
digitalWrite(motor_pin_3, HIGH);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
case 4: // 0010
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, HIGH);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
case 5: // 0011
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, HIGH);
|
||||
digitalWrite(motor_pin_4, HIGH);
|
||||
break;
|
||||
case 6: //0001
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, HIGH);
|
||||
break;
|
||||
case 7: //1001
|
||||
digitalWrite(motor_pin_1, HIGH);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, HIGH);
|
||||
break;
|
||||
case 8: //0000
|
||||
digitalWrite(motor_pin_1, LOW);
|
||||
digitalWrite(motor_pin_2, LOW);
|
||||
digitalWrite(motor_pin_3, LOW);
|
||||
digitalWrite(motor_pin_4, LOW);
|
||||
break;
|
||||
|
||||
|
||||
|
||||
}
|
||||
// }
|
||||
//
|
||||
// if (this->pin_count == 5) {
|
||||
// switch (thisStep) {
|
||||
// case 0: // 01101
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// digitalWrite(motor_pin_3, HIGH);
|
||||
// digitalWrite(motor_pin_4, LOW);
|
||||
// digitalWrite(motor_pin_5, HIGH);
|
||||
// break;
|
||||
// case 1: // 01001
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// digitalWrite(motor_pin_3, LOW);
|
||||
// digitalWrite(motor_pin_4, LOW);
|
||||
// digitalWrite(motor_pin_5, HIGH);
|
||||
// break;
|
||||
// case 2: // 01011
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// digitalWrite(motor_pin_3, LOW);
|
||||
// digitalWrite(motor_pin_4, HIGH);
|
||||
// digitalWrite(motor_pin_5, HIGH);
|
||||
// break;
|
||||
// case 3: // 01010
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// digitalWrite(motor_pin_3, LOW);
|
||||
// digitalWrite(motor_pin_4, HIGH);
|
||||
// digitalWrite(motor_pin_5, LOW);
|
||||
// break;
|
||||
// case 4: // 11010
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, HIGH);
|
||||
// digitalWrite(motor_pin_3, LOW);
|
||||
// digitalWrite(motor_pin_4, HIGH);
|
||||
// digitalWrite(motor_pin_5, LOW);
|
||||
// break;
|
||||
// case 5: // 10010
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// digitalWrite(motor_pin_3, LOW);
|
||||
// digitalWrite(motor_pin_4, HIGH);
|
||||
// digitalWrite(motor_pin_5, LOW);
|
||||
// break;
|
||||
// case 6: // 10110
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// digitalWrite(motor_pin_3, HIGH);
|
||||
// digitalWrite(motor_pin_4, HIGH);
|
||||
// digitalWrite(motor_pin_5, LOW);
|
||||
// break;
|
||||
// case 7: // 10100
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// digitalWrite(motor_pin_3, HIGH);
|
||||
// digitalWrite(motor_pin_4, LOW);
|
||||
// digitalWrite(motor_pin_5, LOW);
|
||||
// break;
|
||||
// case 8: // 10101
|
||||
// digitalWrite(motor_pin_1, HIGH);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// digitalWrite(motor_pin_3, HIGH);
|
||||
// digitalWrite(motor_pin_4, LOW);
|
||||
// digitalWrite(motor_pin_5, HIGH);
|
||||
// break;
|
||||
// case 9: // 00101
|
||||
// digitalWrite(motor_pin_1, LOW);
|
||||
// digitalWrite(motor_pin_2, LOW);
|
||||
// digitalWrite(motor_pin_3, HIGH);
|
||||
// digitalWrite(motor_pin_4, LOW);
|
||||
// digitalWrite(motor_pin_5, HIGH);
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
/*
|
||||
version() returns the version of the library:
|
||||
*/
|
||||
int Stepper::version(void)
|
||||
{
|
||||
return 5;
|
||||
}
|
||||
121
src/modules/stage/StepperF_alt.h
Normal file
121
src/modules/stage/StepperF_alt.h
Normal file
|
|
@ -0,0 +1,121 @@
|
|||
/*
|
||||
* Stepper.h - Stepper library for Wiring/Arduino - Version 1.1.0
|
||||
*
|
||||
* Original library (0.1) by Tom Igoe.
|
||||
* Two-wire modifications (0.2) by Sebastian Gassner
|
||||
* Combination version (0.3) by Tom Igoe and David Mellis
|
||||
* Bug fix for four-wire (0.4) by Tom Igoe, bug fix from Noah Shibley
|
||||
* High-speed stepping mod by Eugene Kozlenko
|
||||
* Timer rollover fix by Eugene Kozlenko
|
||||
* Five phase five wire (1.1.0) by Ryan Orendorff
|
||||
*
|
||||
* 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 2.1 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
|
||||
*
|
||||
*
|
||||
* Drives a unipolar, bipolar, or five phase stepper motor.
|
||||
*
|
||||
* When wiring multiple stepper motors to a microcontroller, you quickly run
|
||||
* out of output pins, with each motor requiring 4 connections.
|
||||
*
|
||||
* By making use of the fact that at any time two of the four motor coils are
|
||||
* the inverse of the other two, the number of control connections can be
|
||||
* reduced from 4 to 2 for the unipolar and bipolar motors.
|
||||
*
|
||||
* A slightly modified circuit around a Darlington transistor array or an
|
||||
* L293 H-bridge connects to only 2 microcontroler pins, inverts the signals
|
||||
* received, and delivers the 4 (2 plus 2 inverted ones) output signals
|
||||
* required for driving a stepper motor. Similarly the Arduino motor shields
|
||||
* 2 direction pins may be used.
|
||||
*
|
||||
* The sequence of control signals for 5 phase, 5 control wires is as follows:
|
||||
*
|
||||
* Step C0 C1 C2 C3 C4
|
||||
* 1 0 1 1 0 1
|
||||
* 2 0 1 0 0 1
|
||||
* 3 0 1 0 1 1
|
||||
* 4 0 1 0 1 0
|
||||
* 5 1 1 0 1 0
|
||||
* 6 1 0 0 1 0
|
||||
* 7 1 0 1 1 0
|
||||
* 8 1 0 1 0 0
|
||||
* 9 1 0 1 0 1
|
||||
* 10 0 0 1 0 1
|
||||
*
|
||||
* The sequence of control signals for 4 control wires is as follows:
|
||||
*
|
||||
* Step C0 C1 C2 C3
|
||||
* 1 1 0 1 0
|
||||
* 2 0 1 1 0
|
||||
* 3 0 1 0 1
|
||||
* 4 1 0 0 1
|
||||
*
|
||||
* The sequence of controls signals for 2 control wires is as follows
|
||||
* (columns C1 and C2 from above):
|
||||
*
|
||||
* Step C0 C1
|
||||
* 1 0 1
|
||||
* 2 1 1
|
||||
* 3 1 0
|
||||
* 4 0 0
|
||||
*
|
||||
* The circuits can be found at
|
||||
*
|
||||
* http://www.arduino.cc/en/Tutorial/Stepper
|
||||
*/
|
||||
|
||||
// ensure this library description is only included once
|
||||
#ifndef Stepper_h
|
||||
#define Stepper_h
|
||||
|
||||
// library interface description
|
||||
class Stepper {
|
||||
public:
|
||||
// constructors:
|
||||
// Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2);
|
||||
Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2,
|
||||
int motor_pin_3, int motor_pin_4);
|
||||
// Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2,
|
||||
// int motor_pin_3, int motor_pin_4,
|
||||
// int motor_pin_5);
|
||||
|
||||
// speed setter method:
|
||||
void setSpeed(long whatSpeed);
|
||||
|
||||
// mover method:
|
||||
void step(int number_of_steps);
|
||||
void stepMotor(int this_step); // made public by RWB for easier multi-stepper control
|
||||
|
||||
int version(void);
|
||||
|
||||
private:
|
||||
|
||||
int direction; // Direction of rotation
|
||||
unsigned long step_delay; // delay between steps, in ms, based on speed
|
||||
int number_of_steps; // total number of steps this motor can take
|
||||
int pin_count; // how many pins are in use.
|
||||
int step_number; // which step the motor is on
|
||||
|
||||
// motor pin numbers:
|
||||
int motor_pin_1;
|
||||
int motor_pin_2;
|
||||
int motor_pin_3;
|
||||
int motor_pin_4;
|
||||
int motor_pin_5; // Only 5 phase motor
|
||||
|
||||
unsigned long last_step_time; // time stamp in us of when the last step was taken
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
283
src/modules/stage/stage.cpp
Normal file
283
src/modules/stage/stage.cpp
Normal file
|
|
@ -0,0 +1,283 @@
|
|||
#include "stage.h"
|
||||
#include "config.h"
|
||||
#include <limits.h>
|
||||
#include <Arduino.h>
|
||||
#include <EEPROM.h>
|
||||
#include "main.h"
|
||||
|
||||
#define EACH_MOTOR for (int i = 0; i < n_motors; i++)
|
||||
|
||||
// The array below has 3 stepper objects, for X,Y,Z respectively
|
||||
const int n_motors = 3;
|
||||
long min_step_delay;
|
||||
const int min_step_delay_eeprom = sizeof(long) * n_motors;
|
||||
long ramp_time;
|
||||
const int ramp_time_eeprom = sizeof(long) * (n_motors + 1);
|
||||
const int axis_max_eeprom = sizeof(long) * (n_motors + 2);
|
||||
Stepper *motors[n_motors];
|
||||
signed long current_pos[n_motors];
|
||||
long steps_remaining[n_motors];
|
||||
|
||||
bool test_mode = false;
|
||||
bool stage_moving = false;
|
||||
|
||||
void stage_setup()
|
||||
{
|
||||
// get the stepoper objects from the motor shield objects
|
||||
|
||||
#if defined(SANGABOARDv2)
|
||||
motors[0] = new Stepper(8, 13, 12, 11, 10);
|
||||
motors[1] = new Stepper(8, 9, 8, 7, 6);
|
||||
motors[2] = new Stepper(8, 5, 4, 3, 2);
|
||||
#elif defined(SANGABOARDv3)
|
||||
motors[0] = new Stepper(8, 8, 9, 10, 11);
|
||||
motors[1] = new Stepper(8, 5, 13, 4, 12);
|
||||
motors[2] = new Stepper(8, 6, 7, A5, A4);
|
||||
#endif
|
||||
EACH_MOTOR
|
||||
{
|
||||
motors[i]->setSpeed(10); // as a default set to 10 rpm, though this is ignored...
|
||||
steps_remaining[i] = 0;
|
||||
EEPROM.get(sizeof(long) * i, current_pos[i]); //read last saved position from EEPROM
|
||||
//current_pos[i] = 0; //alternatively, reset on power cycle!
|
||||
}
|
||||
|
||||
EEPROM.get(min_step_delay_eeprom, min_step_delay);
|
||||
if (min_step_delay < 0)
|
||||
{ // -1 seems to be what we get if it's uninitialised.
|
||||
min_step_delay = 1000;
|
||||
EEPROM.put(min_step_delay_eeprom, min_step_delay);
|
||||
}
|
||||
EEPROM.get(ramp_time_eeprom, ramp_time);
|
||||
if (ramp_time < 0)
|
||||
{ // -1 seems to be what we get if it's uninitialised.
|
||||
ramp_time = 0;
|
||||
EEPROM.put(ramp_time_eeprom, ramp_time);
|
||||
}
|
||||
|
||||
register_module(stage_commands, stage_loop);
|
||||
}
|
||||
|
||||
void stepMotor(int motor, long dx)
|
||||
{
|
||||
//make a single step of a single motor.
|
||||
current_pos[motor] += dx;
|
||||
motors[motor]->stepMotor(((current_pos[motor] % 8) + 8) % 8); //forgive the double-modulo; I need 0-7 even for -ve numbers
|
||||
}
|
||||
|
||||
void releaseMotor(int motor)
|
||||
{
|
||||
//release the stepper motor (de-enegrise the coils)
|
||||
motors[motor]->stepMotor(8);
|
||||
}
|
||||
|
||||
void print_position()
|
||||
{
|
||||
EACH_MOTOR
|
||||
{
|
||||
if (i > 0)
|
||||
Serial.print(" ");
|
||||
Serial.print(current_pos[i]);
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
unsigned long move_start_time = 0;
|
||||
unsigned long distance_moved[n_motors];
|
||||
unsigned long displacement[n_motors];
|
||||
float final_scaled_t;
|
||||
float step_delay[n_motors];
|
||||
long move_directions[n_motors];
|
||||
|
||||
void start_move(unsigned long displ[n_motors])
|
||||
{
|
||||
// move all the axes in a nice move
|
||||
// split displacements into magnitude and direction, and find max. travel
|
||||
unsigned long max_steps = 0;
|
||||
EACH_MOTOR
|
||||
{
|
||||
move_directions[i] = displ[i] > 0 ? 1 : -1;
|
||||
displacement[i] = displ[i] * move_directions[i];
|
||||
if (displacement[i] > max_steps)
|
||||
max_steps = displacement[i];
|
||||
}
|
||||
// scale the step delays so the move goes in a straight line, with >=1 motor
|
||||
// running at max. speed.
|
||||
EACH_MOTOR if (displacement[i] > 0)
|
||||
{
|
||||
step_delay[i] = float(max_steps) / float(displacement[i]) * float(min_step_delay);
|
||||
}
|
||||
else
|
||||
{
|
||||
step_delay[i] = 9999999999;
|
||||
}
|
||||
|
||||
EACH_MOTOR distance_moved[i] = 0;
|
||||
move_start_time = micros();
|
||||
final_scaled_t = (float)max_steps * min_step_delay; //NB total time taken will be final_scaled_t + 2*ramp_time
|
||||
stage_moving = true;
|
||||
}
|
||||
|
||||
void stage_loop()
|
||||
{
|
||||
if (!stage_moving)
|
||||
return;
|
||||
|
||||
unsigned long now = micros();
|
||||
float elapsed_t;
|
||||
if (now < move_start_time) //overflow in micros() after ~70 min
|
||||
elapsed_t = (float)(now + (ULONG_MAX - move_start_time));
|
||||
else
|
||||
elapsed_t = (float)(now - move_start_time);
|
||||
|
||||
float scaled_t; //scale time to allow for acceleration
|
||||
if (ramp_time > 0)
|
||||
{
|
||||
// if a ramp time is specified, accelerate at a constant acceleration for the
|
||||
// ramp time, then move at constant (maximum) speed, then decelerate. If the
|
||||
// move is shorter than 2*ramp_time, accelerate then decelerate.
|
||||
float remaining_t = final_scaled_t + ramp_time - elapsed_t;
|
||||
if (elapsed_t < ramp_time && remaining_t > elapsed_t)
|
||||
{ //for the first ramp_time, gradually accelerate
|
||||
scaled_t = elapsed_t * elapsed_t / (2 * ramp_time);
|
||||
}
|
||||
else if (remaining_t < ramp_time)
|
||||
{
|
||||
scaled_t = final_scaled_t - remaining_t * remaining_t / (2 * ramp_time);
|
||||
}
|
||||
else
|
||||
{
|
||||
scaled_t = elapsed_t - ramp_time / 2;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
scaled_t = elapsed_t;
|
||||
}
|
||||
stage_moving = false;
|
||||
EACH_MOTOR
|
||||
{
|
||||
if (distance_moved[i] < displacement[i])
|
||||
{
|
||||
stage_moving = true; //only if all axes are done are we truly finished.
|
||||
|
||||
// check if it's time to take another step and move if needed.
|
||||
if (scaled_t > ((float)distance_moved[i] + 0.5) * step_delay[i])
|
||||
{
|
||||
stepMotor(i, move_directions[i]);
|
||||
distance_moved[i]++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stage_move_single_axis(uint8_t axis, String command)
|
||||
{
|
||||
String args[1];
|
||||
parse_arguments(args, command, 1);
|
||||
int move = args[0].toInt();
|
||||
EACH_MOTOR displacement[i] = 0;
|
||||
displacement[axis] = move;
|
||||
start_move(displacement);
|
||||
}
|
||||
|
||||
void stage_mrx(String command)
|
||||
{
|
||||
stage_move_single_axis(0, command);
|
||||
}
|
||||
|
||||
void stage_mry(String command)
|
||||
{
|
||||
stage_move_single_axis(1, command);
|
||||
}
|
||||
void stage_mrz(String command)
|
||||
{
|
||||
stage_move_single_axis(2, command);
|
||||
}
|
||||
|
||||
void stage_mr(String command)
|
||||
{
|
||||
String args[3];
|
||||
parse_arguments(args, command, 3);
|
||||
EACH_MOTOR
|
||||
{
|
||||
displacement[i] = args[i].toInt();
|
||||
}
|
||||
|
||||
start_move(displacement);
|
||||
}
|
||||
|
||||
void stage_release(String command)
|
||||
{
|
||||
EACH_MOTOR
|
||||
{
|
||||
releaseMotor(i);
|
||||
}
|
||||
}
|
||||
|
||||
void stage_p(String command)
|
||||
{
|
||||
print_position();
|
||||
}
|
||||
|
||||
void stage_min_step_delay(String command)
|
||||
{
|
||||
String args[1];
|
||||
parse_arguments(args, command, 1);
|
||||
if (args[0].equals("?"))
|
||||
{
|
||||
Serial.print("minimum step delay ");
|
||||
Serial.println(min_step_delay);
|
||||
}
|
||||
else
|
||||
{
|
||||
min_step_delay = args[0].toInt();
|
||||
EEPROM.put(min_step_delay_eeprom, min_step_delay);
|
||||
Serial.println("done.");
|
||||
}
|
||||
}
|
||||
|
||||
void stage_ramp_time(String command)
|
||||
{
|
||||
String args[1];
|
||||
parse_arguments(args, command, 1);
|
||||
if (args[0].equals("?"))
|
||||
{
|
||||
Serial.print("ramp_time ");
|
||||
Serial.println(ramp_time);
|
||||
}
|
||||
else
|
||||
{
|
||||
ramp_time = args[0].toInt();
|
||||
EEPROM.put(ramp_time_eeprom, ramp_time);
|
||||
Serial.println("done.");
|
||||
}
|
||||
}
|
||||
void stage_zero(String command)
|
||||
{
|
||||
EACH_MOTOR current_pos[i] = 0;
|
||||
Serial.println(F("position reset to 0 0 0"));
|
||||
EEPROM.put(0, current_pos);
|
||||
}
|
||||
|
||||
void stage_stop(String command)
|
||||
{
|
||||
stage_moving = false;
|
||||
Serial.println("Move aborted");
|
||||
}
|
||||
|
||||
//TODO: move help strings to program memory
|
||||
//F() cannot be used outside block context though
|
||||
extern const Command stage_commands[] = {
|
||||
{"mrx", stage_mrx},
|
||||
{"mry", stage_mry},
|
||||
{"mrz", stage_mrz},
|
||||
{"mr", stage_mr},
|
||||
{"release", stage_release},
|
||||
{"p", stage_p},
|
||||
{"ramp_time", stage_ramp_time},
|
||||
{"min_step_delay", stage_min_step_delay},
|
||||
{"dt", stage_min_step_delay},
|
||||
{"zero", stage_min_step_delay},
|
||||
{"stop", stage_stop},
|
||||
END_COMMAND};
|
||||
24
src/modules/stage/stage.h
Normal file
24
src/modules/stage/stage.h
Normal file
|
|
@ -0,0 +1,24 @@
|
|||
#include "StepperF_alt.h"
|
||||
#include "main.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
#define EACH_MOTOR for (int i = 0; i < n_motors; i++)
|
||||
#define VER_STRING "Sangaboard Firmware v0.6"
|
||||
|
||||
void stage_setup();
|
||||
void stage_loop();
|
||||
|
||||
void print_position();
|
||||
|
||||
void stage_mrx(String command);
|
||||
void stage_mry(String command);
|
||||
void stage_mrz(String command);
|
||||
void stage_mr(String command);
|
||||
void stage_release(String command);
|
||||
void stage_p(String command);
|
||||
void stage_min_step_delay(String command);
|
||||
void stage_ramp_time(String command);
|
||||
void stage_zero(String command);
|
||||
void stage_stop(String command);
|
||||
|
||||
extern const Command stage_commands[];
|
||||
11
test/README
Normal file
11
test/README
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
|
||||
This directory is intended for PlatformIO Unit Testing and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PlatformIO Unit Testing:
|
||||
- https://docs.platformio.org/page/plus/unit-testing.html
|
||||
Loading…
Add table
Add a link
Reference in a new issue