#include <TeensyStep.h> // library for teensy-based motor control: https://luni64.github.io/TeensyStep/documentation/
#include <TimeLib.h>
#include <SD.h>
#include <SPI.h>
const int chipSelect = BUILTIN_SDCARD;
StepControl controller; // controller object (manages individual steppers)
File logfile;
const int indicator = LED_BUILTIN, // light
estop = 17, // input from the e-stop.
log_time = 250, // in ms, how often to report postitions.
sd_write_time = 1000, // defines SD read/write operations, so don't make this too small
motor_steps_per_rev = 200; // how many steps per one motor revolution
int max_speed = 40000, // top speed
acceleration = 20000; // acceleration = steps/sec/sec;
/* const int avg_sens_widths[] = {122, 124, 118, 100, 51, 110}; // rough number of steps it takes to traverse a sensor */
/* const int avg_sens_widths[] = {115, 112, 98, 100, 175, 128}; // rough number of steps it takes to traverse a sensor @ 400ustep */
/* const int avg_sens_widths[] = {460, 448, 392, 400, 700, 512}; // rough number of steps it takes to traverse a sensor @ 1600ustep */
const int avg_sens_widths[] = {14720, 14336, 12544, 12800, 22400, 16384}; // rough number of steps it takes to traverse a sensor @ 128kustep
int activeletter = 4; // for testing purposes
String command = ""; // logical control
// const long whole_revs[] = { 4572, 9440, 2229, 6983, 400, 5082 }; // measured manually on the mockup
/* long whole_revs[] = { 29128, 60082, 14060, 44378, 1091, 29880 }; // real build, measured manually */
/* long whole_revs[] = { 116512, 240328, 56240, 177512, 6000, 119520 }; // real build, measured * 4 for 1600ustep */
long whole_revs[] = { 3728384, 7690496, 1799680, 5680384, 192000, 3824640 };
const long offsets[] = {0, 0, 0, 0, 0, 0}; // distance from sensor 0 to home position
const int revolutions[] = {6, 3, 12, 4, 2, 6}; // number of times to revolve per cycle
long positions[6];
long prev_positions[6];
long period = 30; // unused
long targets[6]; // to compare with positions[]
int certainties[] = { 0, 0, 0, 0, 0, 0 };
boolean searching = false;
boolean flips[] = {false, true, true, false, false, false};
float thetas[6];
int speeds[6]; // unused?
/* TEENSY */
int lpins[][5] = { // letter pins. two dimensional array of [letters][pins].
// 0 1 2 3 4
// step, dir, en, sens1, sens2
{2, 3, 4, 33, 34}, // 0
{5, 6, 7, 35, 36}, // 1
{8, 9, 10, 37, 38}, // 2
{11, 12, 24, 39, 14}, // 3
{25, 26, 27, 15, 0}, // 4
{28, 29, 30, 16, 0} // 5
};
/* MEGA */
/* int lpins[][5] = { // letter pins. two dimensional array of [letters][pins]. */
/* // 0 1 2 3 4 */
/* // step, dir, en, sens1, sens2 */
/* {A0, A1, 38, 27, 7}, // 0 */
/* {A6, A7, A2, 33, 4}, // 1 */
/* {46, 48, A8, 2, 22}, // 2 */
/* {26, 28, 24, 50, 5}, // 3 */
/* {36, 34, 30, 3, 0}, // 4 */
/* {47, 32, 45, 35, 0} // 5 */
/* }; */
boolean debug = true, // flag to turn on/off serial output
powertoggle = true, // indicator as to whether we've disabled the motors or not
need_to_log = true,
SDokay = true,
force_log = false,
log_pos = true, sd_log = true; // flags to note whether we've logged positions during moves (either to serial or eventually to SD)
// initiate stepper array (will be populated in setup)
Stepper *steppers[6] = {NULL, NULL, NULL, NULL, NULL, NULL};
void setup() {
setSyncProvider(getTeensy3Time);
if (debug) {
Serial.begin(115200);
while(!Serial); // CAUTION: THIS WILL HANG IF NOT CONNECTED TO COMPUTER
delay(150); // gives time for serial to establish itself.
Serial.println("=================== setup =====================");
if (timeStatus()!= timeSet) {
Serial.println("Unable to sync with the RTC");
} else {
Serial.println("RTC has set the system time");
}
Serial.print("current time: ");
Serial.print(hour());
printDigits(minute());
printDigits(second());
Serial.print(" ");
Serial.print(day());
Serial.print(" ");
Serial.print(month());
Serial.print(" ");
Serial.print(year());
Serial.println();
Serial.print("max speed: ");
Serial.print(max_speed);
Serial.print( " accel: ");
Serial.print(acceleration);
Serial.println();
}
if (debug) Serial.println("---------");
if (!SD.begin(chipSelect)) {
if (debug) Serial.println("SD card initialization failed");
SDokay = false;
}
if (debug) Serial.println("SD card initialization ok");
SD_read_positions();
// initiate steppers
for (int i = 0; i < 6; i++) {
steppers[i] = new Stepper(lpins[i][0], lpins[i][1]);
// set speeds + accelerations
steppers[i]->setMaxSpeed(max_speed);
steppers[i]->setAcceleration(acceleration);
steppers[i]->setPosition(positions[i]);
}
// pin declarations
for (int s = 0; s < 6; s++) {
if (debug) Serial.print("== letter ");
if (debug) Serial.print(s);
if (debug) Serial.println(" ==");
for (int p = 0; p < 5; p++) {
if (lpins[s][p] != 0) {
if (p < 3) {
if (debug) Serial.print("setting pin to output: ");
if (debug) Serial.println(lpins[s][p]);
pinMode(lpins[s][p], OUTPUT);
}
else {
if (debug) Serial.print("setting pin to input_pullup: ");
if (debug) Serial.println(lpins[s][p]);
pinMode(lpins[s][p], INPUT_PULLUP);
}
}
}
digitalWrite(lpins[s][2], HIGH); // enable all motors
}
pinMode(LED_BUILTIN, OUTPUT);
pinMode(estop, INPUT);
if (debug) Serial.println("---------");
if (debug) Serial.print("estop status: ");
if (debug) Serial.println((digitalRead(estop) == HIGH) ? "high" : "low");
while (digitalRead(estop) == LOW) {
if (debug) Serial.println("estop active. doing nothing.");
delay(1000);
} // hang until estop is reset
if (debug) Serial.println("waiting 5secs at end of setup for you to turn on the motors");
for(int i = 5; i > 0; i--) {
if (debug) Serial.println(i);
digitalWrite(indicator, HIGH);
delay(250);
digitalWrite(indicator, LOW);
delay(750);
}
if (debug) Serial.println("ok going...");
} // end setup
void loop() {
if (digitalRead(estop) == LOW) command = "shutoff";
for (int i = 0; i < 6; i++) {
positions[i] = steppers[i]->getPosition();
}
log_position();
/* SERIAL INPUT */
if (debug && Serial.available() > 0) {
int incoming = Serial.read();
Serial.print("input: ");
Serial.print(incoming);
Serial.print("\t letter: ");
Serial.write(incoming);
Serial.println();
if (incoming > 47 && incoming < 54) {
// input is one of 0-5
// subtract 49 because the ascii code for 1 is 49
activeletter = incoming - 48;
if (debug) Serial.print("new active letter: ");
if (debug) Serial.println(activeletter);
}
switch(incoming) {
case 67: // C
command = "calibrate1";
break;
case 99: // c
command = "calibrate2";
break;
case 32: // space
command = "shutoff";
break;
case 104: // h
command = "softstop";
break;
case 114: // r
steppers[activeletter]->setTargetRel(random(-whole_revs[activeletter], whole_revs[activeletter]));
controller.move(*steppers[activeletter]);
break;
case 109: // m
absolutemove(Serial.parseInt(), Serial.parseInt());
break;
case 77: // M
relativemove(Serial.parseInt(), Serial.parseInt());
break;
case 112: // p
command = "powertoggle";
break;
case 120: // x
command = "cycle";
break;
case 88: // X
customcycle(Serial.parseFloat(),
Serial.parseFloat(),
Serial.parseFloat(),
Serial.parseFloat(),
Serial.parseFloat(),
Serial.parseFloat());
case 115: // s
setspeed(Serial.parseInt());
break;
case 97: // a
setaccel(Serial.parseInt());
break;
case 108: // l
report(Serial.parseInt());
break;
case 113: // q
SD_read_positions();
for (int i = 0; i < 6; i++) {
steppers[i]->setPosition(positions[i]);
}
break;
case 81: // Q
force_log = true;
log_position();
break;
case 119: // w
overwriteposition(Serial.parseInt(), Serial.parseInt());
break;
case 87: // W
if (debug) Serial.println("reset all to zero:");
for(int i = 0; i < 6; i++) {
overwriteposition(i, 0);
}
break;
}
}
/* COMMANDS */
if (command == "calibrate1") {
calibrate1(activeletter);
}
else if (command == "calibrate2") {
calibrate2(activeletter);
}
else if (command == "softstop") {
if (debug) Serial.println("soft stopping...");
controller.stop();
if (debug) Serial.print("final positions: ");
for (int i = 0; i < 6; i++) {
if (debug) Serial.print(steppers[i]->getPosition());
if (debug) Serial.print("\t");
}
if (debug) Serial.println();
force_log = true;
log_position();
}
else if (command == "powertoggle") {
powertoggle = !powertoggle;
for (int i = 0; i < 6; i++) {
digitalWrite(lpins[i][2], powertoggle);
}
if (debug) Serial.print("power toggled. enable pins ");
if (debug) Serial.println((powertoggle) ? "ON" : "OFF");
}
else if (command == "shutoff") {
if (debug) Serial.print("POWER SHUTOFF! checking again in ");
for (int i = 0; i < 6; i++) {
digitalWrite(lpins[i][2], LOW);
}
controller.emergencyStop();
powertoggle = false;
for (int i = 3; i > 0; i--) {
if (debug) Serial.print(i);
if (debug) Serial.print("... ");
delay(1000);
}
if (debug) Serial.println();
}
else if (command == "cycle") {
if (debug) Serial.println("starting cycle!");
startnewcycle();
}
command = "";
}
/* MAIN CYCLE: */
void startnewcycle() {
for (int i = 0; i < 6; i++) {
targets[i] = whole_revs[i]*revolutions[i];
steppers[i]->setTargetAbs(targets[i]);
need_to_log = true;
if (!controller.isRunning()) controller.moveAsync(steppers);
}
}
/* ---------------------- */
time_t getTeensy3Time()
{
return Teensy3Clock.get();
}
void printDigits(int digits){
// utility function for digital clock display: prints preceding colon and leading 0
Serial.print(":");
if(digits < 10)
Serial.print('0');
Serial.print(digits);
}
void SD_read_positions() {
logfile = SD.open("positions.txt", FILE_READ);
if (!logfile){
if (debug) Serial.println("can't open position log file");
}
else {
if (debug) Serial.println("SD positions log file opened ok");
}
if (debug) Serial.println("----------");
if (logfile.available()) {
if (debug) Serial.print("positions: ");
for (int i = 0; i < 6; i++) {
positions[i] = logfile.parseInt();
if (debug) Serial.print(positions[i]);
if (debug) Serial.print("\t");
}
if (debug) Serial.println("// EOF");
}
logfile.close();
}
void relativemove(int m, long tgt) {
if (debug) Serial.print("relative move: motor ");
if (debug) Serial.print(m);
if (debug) Serial.print(", amount: ");
if (debug) Serial.println(tgt);
steppers[m]->setTargetRel(tgt);
controller.moveAsync(*steppers[m]);
need_to_log = true;
}
void absolutemove(int m, long tgt) {
if (debug) Serial.print("absolute move: motor ");
if (debug) Serial.print(m);
if (debug) Serial.print(", target: ");
if (debug) Serial.println(tgt);
steppers[m]->setTargetAbs(tgt);
controller.moveAsync(*steppers[m]);
need_to_log = true;
}
void setspeed(int s) {
max_speed = s;
if (debug) Serial.print("speed set to ");
if (debug) Serial.println(s);
for (int i = 0; i < 6; i++) {
steppers[i]->setMaxSpeed(max_speed);
}
}
void setaccel(int a) {
if (debug) Serial.print("acceleration set to ");
if (debug) Serial.println(a);
acceleration = a;
for (int i = 0; i < 6; i++) {
steppers[i]->setAcceleration(acceleration);
}
}
void report(int m) {
if (debug) Serial.print("position of motor ");
if (debug) Serial.print(m);
if (debug) Serial.print(": ");
if (debug) Serial.println(steppers[m]->getPosition());
}
void customcycle(float a, float b, float c, float d, float e, float f) {
float fracs[] = {a, b, c, d, e, f};
if (debug) Serial.println("custom cycle:");
for (int i = 0; i < 6; i++) {
if (debug) Serial.print(fracs[i]);
if (debug) Serial.print("\t");
}
if (debug) Serial.println();
if (debug) Serial.print("targets: ");
for (int i = 0; i < 6; i++) {
targets[i] = whole_revs[i]*fracs[i];
steppers[i]->setTargetAbs(targets[i]);
need_to_log = true;
if (debug) Serial.print(targets[i]);
if (debug) Serial.print("\t");
}
if (debug) Serial.println();
if (debug) Serial.println("running");
controller.moveAsync(steppers);
}
void overwriteposition(int m, long pos) {
if (debug) Serial.print("overwriting motor ");
if (debug) Serial.print(m);
if (debug) Serial.print(" to ");
if (debug) Serial.print(pos);
if (debug) Serial.print(". confirmed at:");
steppers[m]->setPosition(pos);
positions[m] = pos;
need_to_log = true;
sd_log = true;
if (debug) Serial.println(steppers[m]->getPosition());
}
void log_position() {
// logs the position once every so often (interval defined global log_time variable)
int sum = 0;
if (force_log) {
need_to_log = true;
sd_log = true;
}
if (need_to_log) {
if ((millis() % log_time < log_time/3 && log_pos == true) || force_log == true) {
log_pos = false;
if (debug) Serial.println("need to log");
/* if (debug && sense(E_pins[3]) && sense(E_pins[4])) Serial.print("------->"); */
if (debug) Serial.print("positions: ");
if (debug) {
for (int i = 0; i < 6; i++) {
Serial.print(positions[i]);
if (i < 5) Serial.print("\t");
}
Serial.println();
}
if ((millis() % sd_write_time < sd_write_time/3 && sd_log == true) || force_log == true) {
if (debug) Serial.println("-- logging to SD card --");
sd_log = false;
logfile = SD.open("positions.txt", FILE_WRITE);
if (logfile) {
logfile.seek(0);
logfile.truncate();
for (int i = 0; i < 6; i++) {
logfile.print(positions[i]);
logfile.print((i < 5) ? ',' : '\n');
}
logfile.close();
}
}
for (int i = 0; i < 6; i++) {
sum += (prev_positions[i] == positions[i]) ? 0 : 1;
prev_positions[i] = positions[i];
}
if (sum == 0) {
sd_log = false;
need_to_log = false;
}
else {
sd_log = true;
need_to_log = true;
}
}
if (millis() % log_time > log_time*2/3) log_pos = true;
if (millis() % sd_write_time > sd_write_time*2/3) sd_log = true;
}
force_log = false;
}