You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
178 lines
5.0 KiB
178 lines
5.0 KiB
4 years ago
|
/*
|
||
|
* Blink
|
||
|
* Turns on an LED on for one second,
|
||
|
* then off for one second, repeatedly.
|
||
|
*/
|
||
|
|
||
|
#include <Arduino.h>
|
||
|
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||
|
#include "pins/pins_TRIGORILLA_14.h"
|
||
|
#elif defined(__AVR_ATmega328P__)
|
||
|
#define LED_PIN LED_BUILTIN
|
||
|
#define X_DIR_PIN PD2
|
||
|
#define X_STEP_PIN PD3
|
||
|
#define X_ENABLE_PIN A0
|
||
|
#define Y_DIR_PIN PD4
|
||
|
#define Y_STEP_PIN PD5
|
||
|
#define Y_ENABLE_PIN A1
|
||
|
#define Z_DIR_PIN PD6
|
||
|
#define Z_STEP_PIN PD7
|
||
|
#define Z_ENABLE_PIN A2
|
||
|
#define E0_DIR_PIN PB0
|
||
|
#define E0_STEP_PIN PB1
|
||
|
#define E0_ENABLE_PIN A3
|
||
|
#define E1_DIR_PIN PB2
|
||
|
#define E1_STEP_PIN PB3
|
||
|
#define E1_ENABLE_PIN A4
|
||
|
#endif
|
||
|
|
||
|
#include "stepper.h"
|
||
|
|
||
|
const int NUM_STEPPER = 5;
|
||
|
|
||
|
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||
|
TimerStepper stepper[NUM_STEPPER];
|
||
|
#elif defined(__AVR_ATmega328P__)
|
||
|
LoopStepper stepper[NUM_STEPPER];
|
||
|
#endif
|
||
|
|
||
|
unsigned long lastMillis = 0;
|
||
|
unsigned long lastCommand = 0;
|
||
|
const long interval = 1000; // interval at which to blink (milliseconds)
|
||
|
int enable_pin[NUM_STEPPER] = {X_ENABLE_PIN, Y_ENABLE_PIN, Z_ENABLE_PIN, E0_ENABLE_PIN, E1_ENABLE_PIN};
|
||
|
|
||
|
|
||
|
void setup()
|
||
|
{
|
||
|
Serial.begin(115200);
|
||
|
/* delay(500);
|
||
|
Serial.println("##########");
|
||
|
Serial.println("MIDI");
|
||
|
Serial.println("##########");
|
||
|
Serial.flush();
|
||
|
*/
|
||
|
// initialize LED digital pin as an output.
|
||
|
pinMode(LED_PIN, OUTPUT);
|
||
|
digitalWrite(LED_PIN, HIGH);
|
||
|
|
||
|
pinMode(TRIGORILLA_HEATER_0_PIN, OUTPUT);
|
||
|
digitalWrite(TRIGORILLA_HEATER_0_PIN, HIGH);
|
||
|
|
||
|
stepper[0].begin( X_DIR_PIN, X_STEP_PIN, X_ENABLE_PIN);
|
||
|
stepper[1].begin( Y_DIR_PIN, Y_STEP_PIN, Y_ENABLE_PIN);
|
||
|
stepper[2].begin( Z_DIR_PIN, Z_STEP_PIN, Z_ENABLE_PIN);
|
||
|
stepper[3].begin(E0_DIR_PIN, E0_STEP_PIN, E0_ENABLE_PIN);
|
||
|
stepper[4].begin(E1_DIR_PIN, E1_STEP_PIN, E1_ENABLE_PIN);
|
||
|
|
||
|
for (int i = 0; i < NUM_STEPPER; i++) {
|
||
|
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||
|
stepper[i].setMaxSpeed(8000);
|
||
|
#elif defined(__AVR_ATmega328P__)
|
||
|
stepper[i].setMaxSpeed(1400);
|
||
|
#endif
|
||
|
stepper[i].setAcceleration(4000);
|
||
|
}
|
||
|
|
||
|
// Timer0 is already used for millis() - we'll just interrupt somewhere
|
||
|
// in the middle and call the "Compare A" function below
|
||
|
OCR0A = 0xAF;
|
||
|
TIMSK0 |= _BV(OCIE0A);
|
||
|
}
|
||
|
|
||
|
// Interrupt is called once a millisecond,
|
||
|
ISR(TIMER0_COMPA_vect) {
|
||
|
for (int i=0; i<NUM_STEPPER; i++) stepper[i].loop();
|
||
|
}
|
||
|
|
||
|
bool parseCmd(char* buf, int values[4]) {
|
||
|
char cmd;
|
||
|
int n = sscanf(buf, "%c %d,%d,%d,%d", &cmd, &values[0], &values[1], &values[2], &values[3]);
|
||
|
if (n == 5) return true;
|
||
|
else return false;
|
||
|
}
|
||
|
|
||
|
void loop()
|
||
|
{
|
||
|
unsigned long currentMillis = millis();
|
||
|
|
||
|
if(Serial.available()) {
|
||
|
String command = Serial.readStringUntil('\n');
|
||
|
lastCommand = millis();
|
||
|
if (command.equals("ENABLE")) {
|
||
|
stepper[0].enable();
|
||
|
stepper[1].enable();
|
||
|
stepper[2].enable();
|
||
|
stepper[3].enable();
|
||
|
//Serial.println("## ENABLE");
|
||
|
} else if (command.equals("DISABLE")) {
|
||
|
stepper[0].disable();
|
||
|
stepper[1].disable();
|
||
|
stepper[2].disable();
|
||
|
stepper[3].disable();
|
||
|
//Serial.println("## DISABLE");
|
||
|
} else if (command.charAt(0) == 'A') {
|
||
|
int values[4];
|
||
|
char* buf = command.c_str();
|
||
|
bool success = parseCmd(buf, values);
|
||
|
if (success) {
|
||
|
stepper[0].setAcceleration(values[0]);
|
||
|
stepper[1].setAcceleration(values[1]);
|
||
|
stepper[2].setAcceleration(values[2]);
|
||
|
stepper[3].setAcceleration(values[3]);
|
||
|
}
|
||
|
} else if (command.charAt(0) == 'S') {
|
||
|
int values[4];
|
||
|
char* buf = command.c_str();
|
||
|
bool success = parseCmd(buf, values);
|
||
|
if (success) {
|
||
|
stepper[0].setSpeedDirect(values[0]);
|
||
|
stepper[1].setSpeedDirect(values[1]);
|
||
|
stepper[2].setSpeedDirect(values[2]);
|
||
|
stepper[3].setSpeedDirect(values[3]);
|
||
|
Serial.print("Motor Speeds: ");
|
||
|
Serial.print(values[0]); Serial.print(", ");
|
||
|
Serial.print(values[1]); Serial.print(", ");
|
||
|
Serial.print(values[2]); Serial.print(", ");
|
||
|
Serial.print(values[3]); Serial.print("\n");
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
if (millis() - lastCommand > 5000) {
|
||
|
stepper[0].disable();
|
||
|
stepper[1].disable();
|
||
|
stepper[2].disable();
|
||
|
stepper[3].disable();
|
||
|
}
|
||
|
/*
|
||
|
stepper[0].setSpeed(-4000);
|
||
|
stepper[1].setSpeed(-4000);
|
||
|
stepper[2].setSpeed(4000);
|
||
|
stepper[3].setSpeed(4000);
|
||
|
delay(2000);
|
||
|
stepper[0].setSpeed(0);
|
||
|
stepper[1].setSpeed(0);
|
||
|
stepper[2].setSpeed(0);
|
||
|
stepper[3].setSpeed(0);
|
||
|
while (stepper[0].isAccelerating() || stepper[1].isAccelerating() || stepper[2].isAccelerating() || stepper[3].isAccelerating()) {
|
||
|
delay(20);
|
||
|
}
|
||
|
stepper[0].setSpeed(2400);
|
||
|
stepper[1].setSpeed(2400);
|
||
|
stepper[2].setSpeed(2400);
|
||
|
stepper[3].setSpeed(2400);
|
||
|
delay(4200);
|
||
|
stepper[0].setSpeed(0);
|
||
|
stepper[1].setSpeed(0);
|
||
|
stepper[2].setSpeed(0);
|
||
|
stepper[3].setSpeed(0);
|
||
|
while (stepper[0].isAccelerating() || stepper[1].isAccelerating() || stepper[2].isAccelerating() || stepper[3].isAccelerating()) {
|
||
|
delay(20);
|
||
|
}
|
||
|
// for (int i=0; i<NUM_STEPPER; i++) {
|
||
|
// stepper[i].disable();
|
||
|
// }
|
||
|
// delay(2000);
|
||
|
*/
|
||
|
}
|