#define ENA 3
#define MS1A 4
#define MS2A 5
#define STEPA 6
#define DIRA 7
#define ENB 8
#define MS1B 9
#define MS2B 10
#define STEPB 11
#define DIRB 12
#include <LiquidCrystal.h>
#include <NativeEthernet.h>
#include "Arduino.h"
#include "TeensyTimerTool.h"
#include <atomic>
const int rs = 27, en = 28, d4 = 29, d5 = 30, d6 = 31, d7 = 32;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};
IPAddress ip(192, 168, 1, 177);
uint16_t localPort = 8888; // local port to listen on
EthernetLinkStatus link_status = Unknown;
EthernetUDP Udp;
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; // buffer to hold incoming packet,
TeensyTimerTool::OneShotTimer timer1(TeensyTimerTool::GPT1); // generate a timer from the pool (Pool: 2xGPT, 16xTMR(QUAD), 20xTCK)
TeensyTimerTool::OneShotTimer timer2(TeensyTimerTool::GPT2); // generate a timer from the pool (Pool: 2xGPT, 16xTMR(QUAD), 20xTCK)
#define V_MAX ((float)(0.25*2048.0*64.0*1e-6))
#define A_MAX ((float)(V_MAX*1e-6))
float ComputeV(float v,float a) {
if (a > 0.0f) {
if (a > A_MAX) a = A_MAX;
const float v_half = 0.5f * v;
if (v > 0.0f) { // acceleration
return std::min(V_MAX, v_half + sqrtf(v_half*v_half + a));
}
if (v < 0.0f) { // deceleration
const float discr = v_half*v_half - a;
if (discr >= 0.0f) {
return v_half - sqrtf(discr);
}
// change of direction
const float a_inv = 1.0f / a; // > 0
const float h = v_half * a_inv; // < 0
return 1.0f / (sqrtf(h*h + a_inv) - h);
}
return sqrtf(a);
}
if (a < 0.0f) {
if (a < -A_MAX) a = -A_MAX;
const float v_half = 0.5f * v;
if (v < 0.0f) { // acceleration
return std::max(-V_MAX, v_half - sqrtf(v_half*v_half - a));
}
if (v > 0.0f) { // deceleration
const float discr = v_half*v_half + a;
if (discr >= 0.0f) {
return v_half + sqrtf(discr);
}
// change of direction
const float a_inv = 1.0f / a; // < 0
const float h = v_half * a_inv; // < 0
return 1.0f / (h - sqrtf(h*h - a_inv));
}
return -sqrt(-a);
}
// constant speed
return v;
}
unsigned int pos1 = 0;
uint32_t last_micros;
double v1 = 0; // steps/microsecond
volatile std::atomic<float> a;
float next_v = 0.f;
void callback1() {
next_v = ComputeV(next_v,a);
if (next_v > 0.f) {
digitalWriteFast(DIRA,1);
digitalWriteFast(STEPA,1);
pos1++;
last_micros = last_micros + (0.5f + 1.f/next_v);
const uint32_t curr_micros = micros();
const int32_t trigger_delay = last_micros-curr_micros;
if (trigger_delay <= 0) timer1.trigger(1);
else timer1.trigger(trigger_delay);
digitalWriteFast(STEPA,0);
} else if (next_v < 0.f) {
digitalWriteFast(DIRA,0);
digitalWriteFast(STEPA,1);
pos1--;
last_micros = last_micros + (0.5f - 1.f/next_v);
const uint32_t curr_micros = micros();
const int32_t trigger_delay = last_micros-curr_micros;
if (trigger_delay <= 0) timer1.trigger(1);
else timer1.trigger(trigger_delay);
digitalWriteFast(STEPA,0);
} else {
timer1.trigger(1);
}
}
unsigned int pos2 = 0;
bool step_pin_high2 = false;
void callback2() {
step_pin_high2 = !step_pin_high2;
digitalWriteFast(STEPB,step_pin_high2);
if (step_pin_high2) {
pos2++;
timer2.trigger(1);
} else {
timer2.trigger(529);
}
}
void setup()
{
Ethernet.begin(mac, ip);
if (Ethernet.hardwareStatus() == EthernetNoHardware) {
Serial.println("Ethernet shield was not found. Sorry, can't run without hardware. :(");
while (true) {
delay(1); // do nothing, no point running without Ethernet hardware
}
}
Serial.println("Ethernet HW found");
lcd.begin(16, 2);
lcd.print("position");
Serial.println("setting up stepper pins");
// pinMode(LED_BUILTIN, OUTPUT);
// for (int j=0;j<9;j++) pinMode(33+j, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(MS1A, OUTPUT);
pinMode(MS2A, OUTPUT);
pinMode(STEPA, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(MS1B, OUTPUT);
pinMode(MS2B, OUTPUT);
pinMode(STEPB, OUTPUT);
pinMode(DIRB, OUTPUT);
digitalWrite(ENA,1);
digitalWrite(MS1A,0);
digitalWrite(MS2A,1); // damit steuere ich das Microstepping: 64
digitalWrite(DIRA, 0);
digitalWrite(STEPA, 1);
digitalWrite(ENA,0);
digitalWrite(ENB,1);
digitalWrite(MS1B,0);
digitalWrite(MS2B,1); // damit steuere ich das Microstepping: 64
digitalWrite(DIRB, 0);
digitalWrite(STEPB, 1);
digitalWrite(ENB,0);
Serial.println("triggering timers");
pinMode(LED_BUILTIN,OUTPUT);
timer1.begin(callback1);
timer2.begin(callback2);
timer1.trigger(1000);
timer2.trigger(1000);
a = 0.001*A_MAX;
Serial.println("setup finished");
}
int count = 0;
void loop()
{
if (1) {
Serial.println("checking Ethernet link status...");
EthernetLinkStatus s = Ethernet.linkStatus();
if (s != link_status) {
link_status = s;
lcd.setCursor(13,0);
switch (link_status) {
case LinkON:
lcd.print("ON ");
Serial.println("Ethernet cable connected.");
break;
case LinkOFF:
lcd.print("OFF");
Serial.println("Ethernet cable disconnected.");
break;
default:
lcd.print("???");
Serial.println("weird link status.");
}
}
}
Serial.println("calling Udp.parsePacket()");
// noInterrupts(); // does not help
// int packetSize = Udp.parsePacket();
// interrupts();
Serial.println("Udp.parsePacket() ok");
/*
if (packetSize) {
Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
packetBuffer[packetSize] = '\0';
int value = 0;
int i = (packetBuffer[0] == '-' || packetBuffer[0] == '+') ? 1 : 0;
if (i < packetSize) {
do {
char c = packetBuffer[i];
if (c < '0' || c > '9') goto parse_error;
value = 10*value + (c-'0');
} while (++i < packetSize);
if (packetBuffer[0] == '-') value = -value;
Serial.print("Received ");
Serial.println(value);
value = std::max(std::max(value,-1000),1000);
a = (A_MAX*1e-3)*value;
lcd.setCursor(9,0);
lcd.print(value);
} else {
parse_error:
Serial.print("Received bad data: \"");
Serial.print(packetBuffer);
Serial.println('"');
}
}
*/
// if (++count >= 4) {
// count = 0;
// a = -a;
// }
// digitalWriteFast(LED_BUILTIN, HIGH);
lcd.setCursor(0, 1);
lcd.print(pos1>>6);
lcd.print(" ");
lcd.print(pos2>>6);
delay(1000);
}