147 lines
2.9 KiB
C
147 lines
2.9 KiB
C
#include <stddef.h>
|
|
// #include <stdio.h>
|
|
#include <stdint.h>
|
|
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
|
|
#include "serial.h"
|
|
|
|
/*
|
|
machine variables
|
|
*/
|
|
#define AXIS_COUNT 5
|
|
|
|
// steps per rev * mm per tooth * teeth per rev
|
|
#define STEPS_PER_MM (3200 * 4.77 * 16)
|
|
|
|
#define MAX_MM_PER_SEC 40
|
|
#define MAX_IMMED_MM_PER_SEC 20
|
|
#define ACCEL 10
|
|
|
|
|
|
/*
|
|
state machine variables
|
|
*/
|
|
|
|
uint8_t linebuffer[64];
|
|
|
|
uint8_t state = 0;
|
|
#define STATE_WAIT_FOR_COMMAND 0
|
|
#define STATE_WAIT_FOR_COMMAND_DIGIT 1
|
|
#define STATE_WAIT_FOR_DATA 2
|
|
#define STATE_WAIT_FOR_DATA_DIGIT 3
|
|
uint8_t command;
|
|
uint8_t command_digit;
|
|
uint8_t axis;
|
|
|
|
// volatile uint32_t xpos = 0;
|
|
// volatile uint32_t ypos = 0;
|
|
// volatile uint32_t zpos = 0;
|
|
// volatile uint32_t edelta = 0;
|
|
//
|
|
// uint32_t xtarget = 0;
|
|
// uint32_t ytarget = 0;
|
|
// uint32_t ztarget = 0;
|
|
//
|
|
// uint16_t xspeed = 0;
|
|
// uint16_t yspeed = 0;
|
|
// uint16_t zspeed = 0;
|
|
|
|
struct axis {
|
|
volatile uint32_t pos;
|
|
int32_t target;
|
|
uint32_t newtarget_mantissa;
|
|
uint8_t newtarget_exp;
|
|
uint8_t newtarget_sign;
|
|
uint16_t speed;
|
|
} axes[AXIS_COUNT];
|
|
|
|
uint8_t axis_char_to_id(uint8_t c) {
|
|
if (c >= 'X')
|
|
return c - 'X';
|
|
if (c == 'E')
|
|
return 3;
|
|
if (c == 'T')
|
|
return 4;
|
|
return 255;
|
|
}
|
|
|
|
int main (void)
|
|
{
|
|
// set up STDIN/OUT/ERR
|
|
// stdin = &serio;
|
|
// stdout = &serio;
|
|
// stderr = &serio;
|
|
|
|
// set up serial
|
|
serial_init();
|
|
|
|
// enable interrupts
|
|
sei();
|
|
|
|
for (;;)
|
|
{
|
|
for (;serial_rxchars() == 0;);
|
|
uint8_t c = serial_popchar();
|
|
if (c >= 'a' && c <= 'z')
|
|
c &= ~32;
|
|
// preprocess
|
|
switch (state) {
|
|
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
|
if (c == 'G' || c == 'M')
|
|
state = STATE_WAIT_FOR_COMMAND;
|
|
break;
|
|
case STATE_WAIT_FOR_DATA_DIGIT:
|
|
if (axis_char_to_id(c) < 255)
|
|
state = STATE_WAIT_FOR_DATA;
|
|
break;
|
|
}
|
|
// actuate
|
|
switch (state) {
|
|
case STATE_WAIT_FOR_COMMAND:
|
|
if (c == 'G' || c == 'M') {
|
|
command = c;
|
|
command_digit = 0;
|
|
state = STATE_WAIT_FOR_COMMAND_DIGIT;
|
|
}
|
|
break;
|
|
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
|
if (c >= '0' && c <= '9') {
|
|
command_digit = (command_digit * 10) + (c - '0');
|
|
}
|
|
else {
|
|
state = STATE_WAIT_FOR_DATA;
|
|
}
|
|
break;
|
|
case STATE_WAIT_FOR_DATA:
|
|
if (axis_char_to_id(c) < 255) {
|
|
axis = axis_char_to_id(c);
|
|
axes[axis].newtarget_mantissa = 0;
|
|
axes[axis].newtarget_exp = 0;
|
|
state = STATE_WAIT_FOR_DATA_DIGIT;
|
|
}
|
|
break;
|
|
case STATE_WAIT_FOR_DATA_DIGIT:
|
|
if (c == '-') {
|
|
axes[axis].newtarget_sign = 1;
|
|
}
|
|
if (c >= '0' && c <= '9') {
|
|
axes[axis].newtarget_mantissa = (axes[axis].newtarget_mantissa * 10) + (c - '0');
|
|
if (axes[axis].newtarget_exp)
|
|
axes[axis].newtarget_exp++;
|
|
}
|
|
if (c == '.') {
|
|
axes[axis].newtarget_exp = 1;
|
|
}
|
|
break;
|
|
}
|
|
if (c == 13) {
|
|
uint8_t i;
|
|
for (i = 0; i < AXIS_COUNT; i++) {
|
|
uint32_t n = axes[i].newtarget_mantissa;
|
|
}
|
|
}
|
|
}
|
|
}
|