commit 2f785220644becacee36100d27ee512bf3733051 Author: Filip Ayazi Date: Fri May 21 04:32:21 2021 +0100 Initial commit First and incomplete version of the refactor, stage module works, including aborting moves. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..0f0d740 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/README.md b/README.md new file mode 100644 index 0000000..23c89bf --- /dev/null +++ b/README.md @@ -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 \ No newline at end of file diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -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 diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -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 +#include + +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 diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..84867a1 --- /dev/null +++ b/platformio.ini @@ -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 \ No newline at end of file diff --git a/src/config.h b/src/config.h new file mode 100644 index 0000000..0af06d9 --- /dev/null +++ b/src/config.h @@ -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 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..c4733b7 --- /dev/null +++ b/src/main.cpp @@ -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 +#include +#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 \ No newline at end of file diff --git a/src/main.h b/src/main.h new file mode 100644 index 0000000..c9f9f77 --- /dev/null +++ b/src/main.h @@ -0,0 +1,27 @@ +#ifndef MAIN_H +#include "config.h" +#include +#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 \ No newline at end of file diff --git a/src/modules/help/help.cpp b/src/modules/help/help.cpp new file mode 100644 index 0000000..f8cb110 --- /dev/null +++ b/src/modules/help/help.cpp @@ -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 - relative move in x")); + Serial.println(F("mry - relative move in y")); + Serial.println(F("mrz - relative move in z")); + Serial.println(F("mr - 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 - set the time taken to accelerate/decelerate in us")); + Serial.println(F("min_step_delay - set the minimum time between steps in us.")); + Serial.println(F("dt - 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 - 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 - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position")); + Serial.println(F("home_max - 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 - set maximum positions")); + #endif + + //Serial.println(F("test_mode - set test_mode ")); + Serial.println(F("version - get firmware version string")); + Serial.println(""); + Serial.println("Input Key:"); + Serial.println(F(" - a decimal integer.")); + Serial.println(""); + Serial.println("--END--"); +} \ No newline at end of file diff --git a/src/modules/help/help.h b/src/modules/help/help.h new file mode 100644 index 0000000..a71b1d6 --- /dev/null +++ b/src/modules/help/help.h @@ -0,0 +1,12 @@ +#ifndef HELP_H +#include +#include "main.h" + +void help(String); +void help_setup(); +const struct Command help_commands[] = { + {"help", help}, + END_COMMAND +}; +#define HELP_H +#endif \ No newline at end of file diff --git a/src/modules/stage/StepperF_alt.cpp b/src/modules/stage/StepperF_alt.cpp new file mode 100644 index 0000000..1848691 --- /dev/null +++ b/src/modules/stage/StepperF_alt.cpp @@ -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 + +///* +// * 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; +} diff --git a/src/modules/stage/StepperF_alt.h b/src/modules/stage/StepperF_alt.h new file mode 100644 index 0000000..86616ad --- /dev/null +++ b/src/modules/stage/StepperF_alt.h @@ -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 + diff --git a/src/modules/stage/stage.cpp b/src/modules/stage/stage.cpp new file mode 100644 index 0000000..6a54858 --- /dev/null +++ b/src/modules/stage/stage.cpp @@ -0,0 +1,283 @@ +#include "stage.h" +#include "config.h" +#include +#include +#include +#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}; \ No newline at end of file diff --git a/src/modules/stage/stage.h b/src/modules/stage/stage.h new file mode 100644 index 0000000..ab3d66e --- /dev/null +++ b/src/modules/stage/stage.h @@ -0,0 +1,24 @@ +#include "StepperF_alt.h" +#include "main.h" +#include + +#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[]; \ No newline at end of file diff --git a/test/README b/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/test/README @@ -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