#define SERIAL_DEBUGGING true
#include "logsteppers.h"
#include <AccelStepper.h>
AccelStepper motor1(1, PUL_PIN1, DIR_PIN1);
AccelStepper motor2(1, PUL_PIN2, DIR_PIN2);
// ***********************************
// USER-DEFINED PARAMETERS
// ***********************************
//session length in minutes (12 seconds)
#define SESSION_LENGTH_MINUTES 250
// speed is in STEPS PER SECOND
#define M1_MAX_SPEED 2000
#define M1_MIN_SPEED 294
// speed is in STEPS PER SECOND
#define M2_MAX_SPEED 330
#define M2_MIN_SPEED 67
// M2_MAX_POSITION_STEPS = YY POSITIONS STEPS FROM SENSOR
#define M2_MAX_POSITION_STEPS_A -3000 // PIN Butt 2 (1)
#define M2_MAX_POSITION_STEPS_B -6000 // PIN Butt 3 (5/8)
#define M2_MAX_POSITION_STEPS_C -3000 // PIN Butt 1 (1 1/2)
// M2_MIN_POSITION_STEPS = XX POSITION STEPS FROM SENSOR
#define M2_MIN_POSITION_STEPS_K -500
// choose linear or decay mode for steppers
// 2 = fixed mode
// 1 = decay mode
// 0 = linear mode
#define M1_DECEL_MODE 1
#define M2_DECEL_MODE 1
// M2_xxx_LIMIT_DELAY = DELAY VALUES (IN MILLISECONDS) UPON REACHING MIN/MAX POSITIONS OF M2
#define M2_INITL_LIMIT_DELAY 40000
#define M2_FINAL_LIMIT_DELAY 33
// CALIBRATION MOVEMENT SPEED
#define CALIB_SPEED 5000
// determines halving rate at end of session
float halving_rate = 5.0;
// determines halving rate of limit delay
float limit_delay_halving_rate = 2.0;
/*
What is 'halving rate'?
- Halving rate is how many 50% division of the value will be done upon reaching session end.
-EXAMPLES (Using default values for this code (MAX_SPEED = 100, MIN_SPEED = 0):
- halving rate of 1.0:
@session_end, spd = 0.5 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 50
- halving rate of 2.0:
@session_end, spd = 0.25 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 25
- halving rate of 3.0:
@session_end, spd = 0.125 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 12.5
- halving rate of 4.0:
@session_end, spd = 0.625 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 6.25
- halving rate of 5.0:
@session_end, spd = 0.3125 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 3.125
- halving rate of 6.0:
@session_end, spd = 0.15625 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 1.5625
- halving rate of 7.0:
@session_end, spd = 0.078125 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 0.78125
- halving rate of 8.0:
@session_end, spd = 0.0390625 * (MAX_SPEED-MIN_SPEED) + MIN_SPEED = 0.390625
*/
// ***********************************
// GLOBAL VARIABLES
// ***********************************
long SESSION_LENGTH_MILLIS = 0;
byte dir_flag = 1;
long M2_MAX_POSITION_STEPS = 0;
long M2_MIN_POSITION_STEPS = 0;
long prev_delay_limit_millis = 0;
long delay_computed_value = 0;
byte delay_limit_flagged = 0;
byte test_mode = 0;
float m1_spd = 0;
float m2_spd = 0;
float m1_spd_linear = 0;
float m1_spd_decayed = 0;
float m2_spd_linear = 0;
float m2_spd_decayed = 0;
// ***********************************
// MAIN FUNCTIONS
// ***********************************
void setup() {
// INIT SERIAL COMMS
Serial.begin(1000000);
// INIT DIGITAL PINS
pinMode(STOP_BUTTON_PIN, INPUT_PULLUP);
pinMode(GO1_BUTTON_PIN, INPUT_PULLUP);
pinMode(GO2_BUTTON_PIN, INPUT_PULLUP);
pinMode(GO3_BUTTON_PIN, INPUT_PULLUP);
pinMode(SENSOR_SIGNAL_PIN, INPUT_PULLUP);
pinMode(ENA_PIN1, OUTPUT);
pinMode(ENA_PIN2, OUTPUT);
// SET STEPPER PARAMETERS
motor1.setMaxSpeed(M1_MAX_SPEED);
motor1.setAcceleration(200);
motor2.setMaxSpeed(M2_MAX_SPEED);
motor2.setAcceleration(200);
motors_disable();
SESSION_LENGTH_MILLIS = float(SESSION_LENGTH_MINUTES) * 60000;
//
// Serial.print("SESSION_LENGTH_MILLIS = ");
// Serial.println(SESSION_LENGTH_MILLIS);
Serial.println("Starting loop...");
M2_MIN_POSITION_STEPS = M2_MIN_POSITION_STEPS_K;
if (test_mode == 1) {
mode = READY;
Serial.println("TEST_MODE ACTIVE: INIT TO READY.");
}
}
void loop() {
switch (mode) {
//idle mode, waiting for the stop button to be pressed
case WAIT:
delay(10);
if (stop_button_NOT_pressed()) {
Serial.println("Stop button is not pressed/released");
// switch to ready mode
mode = READY;
//enable motors
motors_enable();
motor2_calibrate();
Serial.println("READY");
}
break;
//ready mode, waiting for the go button
case READY:
{
byte go_state = 0;
if (go1_button_pressed()) {
// PIN 12
go_state = 1;
}
if (go2_button_pressed()) {
// PIN 2
go_state = 2;
}
if (go3_button_pressed()) {
// PIN 4
go_state = 3;
}
if (test_mode == 1) {
go_state = 1;
}
if (go_state) {
Serial.print("Start button is pressed = ");
Serial.println(go_state);
mode = RUNNING;
switch (go_state) {
case 1:
M2_MAX_POSITION_STEPS = M2_MAX_POSITION_STEPS_A;
break;
case 2:
M2_MAX_POSITION_STEPS = M2_MAX_POSITION_STEPS_B;
break;
case 3:
M2_MAX_POSITION_STEPS = M2_MAX_POSITION_STEPS_C;
break;
default:
M2_MAX_POSITION_STEPS = M2_MAX_POSITION_STEPS_A;
break;
}
// store initial time
session_start = millis();
Serial.print("Running session. CurrPos = ");
Serial.println(motor2.currentPosition());
motors_enable();
compute_speed(0);
delay(10);
}
}
break;
case RUNNING:
{
long elapsed_time = millis() - session_start;
long elapsed_delay = 0;
long motor2_position = motor2.currentPosition();
if ((motor2_position >= M2_MIN_POSITION_STEPS) && (dir_flag == 0)) {
dir_flag = 1;
delay_limit_flagged = 1;
if (SERIAL_DEBUGGING) {
Serial.println("MOVE TO MAX");
}
}
if ((motor2_position <= M2_MAX_POSITION_STEPS) && (dir_flag == 1)) {
dir_flag = 0;
delay_limit_flagged = 1;
if (SERIAL_DEBUGGING) {
Serial.println("MOVE TO MIN");
}
}
if (elapsed_time % 1000 == 0) {
compute_speed(elapsed_time);
}
if (delay_limit_flagged == 1) {
delay_computed_value = get_stepper_linear_delay(elapsed_time, M2_INITL_LIMIT_DELAY, M2_FINAL_LIMIT_DELAY);
prev_delay_limit_millis = millis();
delay_limit_flagged = 2;
Serial.println("LIMIT_DELAY STARTED");
} else if (delay_limit_flagged == 2) {
elapsed_delay = millis() - prev_delay_limit_millis;
if (elapsed_delay > delay_computed_value) {
Serial.println("LIMIT_DELAY DONE");
delay_limit_flagged = 0;
}
} else {
motor2.runSpeed();
prev_delay_limit_millis = millis();
}
motor1.runSpeed();
if (SERIAL_DEBUGGING) {
if (elapsed_time % 100 == 0) {
Serial.print(" t:");
Serial.print(elapsed_time);
Serial.print(" m2_p:");
Serial.print(motor2_position);
Serial.print(" m2_d_curr:");
Serial.print(elapsed_delay);
Serial.print(" m2_d_set:");
Serial.print(delay_computed_value);
Serial.print(" m1_ls:");
Serial.print(m1_spd_linear);
Serial.print(" m1_ds:");
Serial.print(m1_spd_decayed);
Serial.print(" m2_ls:");
Serial.print(m2_spd_linear);
Serial.print(" m2_ds:");
Serial.print(m2_spd_decayed);
Serial.println("");
}
}
check_if_session_end(elapsed_time);
check_if_stopped();
}
break;
default:
Serial.println("INVALID MODE");
break;
}
}
// *****************************************
// TERTIARY FUNCTIONS
// *****************************************
void compute_speed(long elapsed_time) {
m1_spd_linear = get_stepper_linear_speed(elapsed_time, M1_MAX_SPEED, M1_MIN_SPEED);
m1_spd_decayed = get_stepper_half_life_speed(elapsed_time, M1_MAX_SPEED, M1_MIN_SPEED);
if (M1_DECEL_MODE == 1) {
m1_spd = (m1_spd_decayed);
} else if (M1_DECEL_MODE == 0) {
m1_spd = (m1_spd_linear);
} else if (M1_DECEL_MODE == 2) {
m1_spd = (M1_MAX_SPEED);
}
m2_spd_linear = get_stepper_linear_speed(elapsed_time, M2_MAX_SPEED, M2_MIN_SPEED);
m2_spd_decayed = get_stepper_half_life_speed(elapsed_time, M2_MAX_SPEED, M2_MIN_SPEED);
if (M2_DECEL_MODE == 1) {
m2_spd = (m2_spd_decayed);
} else if (M2_DECEL_MODE == 0) {
m2_spd = (m2_spd_linear);
} else if (M2_DECEL_MODE == 2) {
m2_spd = (M2_MAX_SPEED);
}
if (dir_flag == 1) {
m2_spd = -m2_spd;
}
motor1.setSpeed(-m1_spd);
motor2.setSpeed(m2_spd);
}
void check_if_session_end(long elapsed_time) {
//monitor session end
if (elapsed_time > SESSION_LENGTH_MILLIS) {
// add auto_home of motor2
// motor2.runToNewPosition(0);
motors_disable();
Serial.println("Session ended");
if (test_mode == 1) {
Serial.println("TEST_MODE ACTIVE: BACK TO READY.");
mode = READY;
} else {
Serial.println("BACK TO WAIT.");
mode = WAIT;
}
}
}
void check_if_stopped() {
//monitoring emergency stop button
if (stop_button_pressed()) {
motors_disable();
Serial.println("Emergency stop!");
if (test_mode == 1) {
Serial.println("TEST_MODE ACTIVE: BACK TO READY.");
mode = READY;
} else {
Serial.println("BACK TO WAIT.");
mode = WAIT;
}
delay(1000);
}
}
// *****************************************
// STEPPER SPEED SCALING FUNCTIONS
// *****************************************
float get_stepper_linear_speed(long elapsed_time, long max_value, long min_value) {
float val = mapfloat(elapsed_time, 0, SESSION_LENGTH_MILLIS, max_value, min_value);
if (val < min_value) {
val = min_value;
} else if (val > max_value) {
val = max_value;
}
val = abs(val);
return val;
}
// ratio = 0.5^(t/k)
// LINK GRAPH FOR REFERENCE:
// https://www.wolframalpha.com/input?i=y+%3D+0.5+%5E+%28x%29%2C+x+%3E+0
float get_stepper_half_life_speed(long elapsed_time, long max_value, long min_value) {
float halving_exp = (float)elapsed_time / (float)SESSION_LENGTH_MILLIS;
float ratio = pow(0.5, halving_exp * halving_rate);
float val = (max_value - min_value) * ratio + min_value;
val = abs(val);
if (val < min_value) {
val = min_value;
} else if (val > max_value) {
val = max_value;
}
return val;
}
// *****************************************
// STEPPER LIMIT DELAY SCALING FUNCTIONS
//******************************************
float get_stepper_linear_delay(long elapsed_time, long initial_value, long final_value) {
float val = mapfloat(elapsed_time, 0, SESSION_LENGTH_MILLIS, initial_value, final_value);
Serial.print("VAL = ");
Serial.println(val);
Serial.print("initial_value = ");
Serial.println(initial_value);
Serial.print("final_value = ");
Serial.println(final_value);
val = abs(val);
if (val < initial_value) {
val = initial_value;
} else if (val > final_value) {
val = final_value;
}
return val;
}
// ratio = 1 - 0.5^(t/k)
// LINK GRAPH FOR REFERENCE:
// https://www.wolframalpha.com/input?i=y+%3D+1+-+0.5+%5E+%28x%29%2C+x+%3E+0
float get_stepper_half_life_delay(long elapsed_time, long initial_value, long final_value) {
float halving_exp = (float)elapsed_time / (float)SESSION_LENGTH_MILLIS;
float ratio = 1 - pow(0.5, halving_exp * halving_rate);
float val = (initial_value - final_value) * ratio + final_value;
val = abs(val);
if (val < initial_value) {
val = initial_value;
} else if (val > final_value) {
val = final_value;
}
return val;
}
// *****************************************
// MISC FUNCTIONS
// *****************************************
float mapfloat(long x, long in_min, long in_max, long out_min, long out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
//motor 2 calibrarion routine
void motor2_calibrate() {
Serial.println("Calibrating...");
//move forwad
motor2.runToNewPosition(300);
//move back at constant speed before the sensor is hit
motor2.setSpeed(CALIB_SPEED);
Serial.println("Looking for zero....");
byte zero_flag = 1;
while (zero_flag) {
// if stopped button is pressed while waiting for sensor detection
if (stop_button_pressed()) {
motors_disable();
mode = WAIT;
Serial.println("Emergency stop!");
delay(1000);
return;
}
motor2.runSpeed();
if (sensor_object_detected()) {
zero_flag = 0;
motor2.setSpeed(0);
motors_disable();
}
}
Serial.println("Zero found");
// zero motor2 position
motor2.setCurrentPosition(0);
// go to motor2 minimum position
motors_enable();
delay(100);
motor2.setSpeed(-CALIB_SPEED);
Serial.println("Seeking XX Steps....");
byte xx_pos_flag = 1;
while (xx_pos_flag) {
motor2.runSpeed();
// if stopped button is pressed while waiting for sensor detection
if (stop_button_pressed()) {
motors_disable();
mode = WAIT;
Serial.println("Emergency stop!");
delay(1000);
return;
}
if (motor2.currentPosition() < M2_MIN_POSITION_STEPS) {
Serial.println("XX Steps reached!");
xx_pos_flag = 0;
motor2.setSpeed(0);
motors_disable();
}
}
}