q3k/sangaboard-firmware
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Initial commit

Browse source 
First and incomplete version of the refactor, stage module works, including aborting moves.
This commit is contained in:
Filip Ayazi 2021-05-21 04:32:21 +01:00
commit 2f78522064
16 changed files with 1264 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/config.h Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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[];