//this is for handling buffered commands with no response
void handle_commands()
{
byte flags = 0;
long x;
long y;
long z;
unsigned long step_delay;
byte cmd;
if (is_playing()) {
while (commandBuffer.remainingCapacity() > 0 && playback_has_next()) {
commandBuffer.append(playback_next());
}
} else {
digitalWrite(DEBUG_PIN,LOW);
}
//do we have any commands?
if (commandBuffer.size() > 0)
{
CircularBuffer::Cursor cursor = commandBuffer.newCursor();
uint16_t index = 0;
cmd = cursor.read_8();
// Do it later if it's a point queueing command and we don't have room yet.
if (cmd == HOST_CMD_QUEUE_POINT_ABS &&
!point_buffer_has_room(POINT_SIZE)) {
return;
}
switch(cmd)
{
case HOST_CMD_QUEUE_POINT_ABS:
x = (long)cursor.read_32();
y = (long)cursor.read_32();
z = (long)cursor.read_32();
step_delay = (unsigned long)cursor.read_32();
queue_absolute_point(x, y, z, step_delay);
break;
case HOST_CMD_SET_POSITION:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
cli();
set_position(LongPoint((long)cursor.read_32(),(long)cursor.read_32(),(long)cursor.read_32()));
sei();
break;
case HOST_CMD_FIND_AXES_MINIMUM:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
//which ones are we going to?
flags = cursor.read_8();
//find them!
seek_minimums(
flags & 1,
flags & 2,
flags & 4,
cursor.read_32(),
cursor.read_16());
break;
case HOST_CMD_FIND_AXES_MAXIMUM:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
//find them!
seek_maximums(
flags & 1,
flags & 2,
flags & 4,
cursor.read_32(),
cursor.read_16());
break;
case HOST_CMD_DELAY:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
//take it easy.
delay(cursor.read_32());
break;
case HOST_CMD_CHANGE_TOOL:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
//extruder, i choose you!
select_tool(cursor.read_8());
break;
case HOST_CMD_WAIT_FOR_TOOL:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
//get your temp in gear, you lazy bum.
//what tool / timeout /etc?
{
uint8_t currentToolIndex = cursor.read_8();
uint16_t toolPingDelay = (uint16_t)cursor.read_16();
uint16_t toolTimeout = (uint16_t)cursor.read_16();
//check to see if its ready now
if (!is_tool_ready(currentToolIndex))
{
//how often to ping?
toolNextPing = millis() + toolPingDelay;
toolTimeoutEnd = millis() + (toolTimeout * 1000);
//okay, put us in ping-tool-until-ready mode
commandMode = COMMAND_MODE_WAIT_FOR_TOOL;
}
}
break;
case HOST_CMD_TOOL_COMMAND:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
send_tool_command(cursor);
break;
case HOST_CMD_ENABLE_AXES:
// Belay until we're at a good location.
if (!is_point_buffer_empty()) { return; }
{
unsigned char param = cursor.read_8();
bool x = (param & 0x01) != 0;
bool y = (param & 0x02) != 0;
bool z = (param & 0x04) != 0;
if ((param & 0x80) != 0) {
// enable axes
enable_steppers(x,y,z);
} else {
// disable axes
disable_steppers(x,y,z);
}
}
break;
default:
digitalWrite(DEBUG_PIN,HIGH);
hostPacket.unsupported();
}
cursor.commit();
}
}
//Read the string and execute instructions
void process_string(uint8_t *instruction) {
uint8_t code;
uint16_t k;
float temp;
//command commands = NULL;
FloatPoint fp;
//the character / means delete block... used for comments and stuff.
if (instruction[0] == '/') {
Serial.println("ok");
return;
}
enable_steppers();
purge_commands(); //clear old commands
parse_commands(instruction); //create linked list of arguments
if (command_exists('G')) {
code = getValue('G');
switch(code) {
case 0: //Rapid Motion
setXYZ(&fp);
set_target(&fp);
r_move(0); //fast motion in all axis
break;
case 1: //Coordinated Motion
setXYZ(&fp);
set_target(&fp);
if (command_exists('F')) _feedrate = getValue('F'); //feedrate persists till changed.
r_move( _feedrate );
break;
case 2://Clockwise arc
case 3://Counterclockwise arc
FloatPoint cent;
float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
//Set fp Values
setXYZ(&fp);
// Centre coordinates are always relative
cent.x = xaxis->current_units + getValue('I');
cent.y = yaxis->current_units + getValue('J');
aX = (xaxis->current_units - cent.x);
aY = (yaxis->current_units - cent.y);
bX = (fp.x - cent.x);
bY = (fp.y - cent.y);
if (code == 2) { // Clockwise
angleA = atan2(bY, bX);
angleB = atan2(aY, aX);
}
else { // Counterclockwise
angleA = atan2(aY, aX);
angleB = atan2(bY, bX);
}
// Make sure angleB is always greater than angleA
// and if not add 2PI so that it is (this also takes
// care of the special case of angleA == angleB,
// ie we want a complete circle)
if (angleB <= angleA) angleB += 2 * M_PI;
angle = angleB - angleA;
radius = sqrt(aX * aX + aY * aY);
length = radius * angle;
int steps, s, step;
steps = (int) ceil(length / curve_section);
FloatPoint newPoint;
for (s = 1; s <= steps; s++) {
step = (code == 3) ? s : steps - s; // Work backwards for CW
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
newPoint.z = zaxis->current_units;
set_target(&newPoint);
// Need to calculate rate for each section of curve
feedrate_micros = (feedrate > 0) ? feedrate : getMaxFeedrate();
// Make step
r_move(feedrate_micros);
}
break;
case 4: //Dwell
//delay((int)getValue('P'));
break;
case 20: //Inches for Units
_units[0] = X_STEPS_PER_INCH;
_units[1] = Y_STEPS_PER_INCH;
_units[2] = Z_STEPS_PER_INCH;
curve_section = CURVE_SECTION_INCHES;
calculate_deltas();
break;
case 21: //mm for Units
_units[0] = X_STEPS_PER_MM;
_units[1] = Y_STEPS_PER_MM;
_units[2] = Z_STEPS_PER_MM;
curve_section = CURVE_SECTION_MM;
calculate_deltas();
break;
case 28: //go home.
set_target(&zeros);
r_move(getMaxFeedrate());
break;
case 30://go home via an intermediate point.
//Set Target
setXYZ(&fp);
set_target(&fp);
//go there.
r_move(getMaxFeedrate());
//go home.
set_target(&zeros);
r_move(getMaxFeedrate());
break;
case 81: // drilling operation
temp = zaxis->current_units;
//Move only in the XY direction
setXYZ(&fp);
set_target(&fp);
zaxis->target_units = temp;
calculate_deltas();
r_move(getMaxFeedrate());
//Drill DOWN
zaxis->target_units = getValue('Z') + ((abs_mode) ? 0 : zaxis->current_units);
calculate_deltas();
r_move(getMaxFeedrate());
//Drill UP
zaxis->target_units = temp;
calculate_deltas();
r_move(getMaxFeedrate());
case 90://Absolute Positioning
abs_mode = true;
break;
case 91://Incremental Positioning
abs_mode = false;
break;
case 92://Set as home
set_position(&zeros);
break;
case 93://Inverse Time Feed Mode
break; //TODO: add this
case 94://Feed per Minute Mode
break; //TODO: add this
default:
Serial.print("huh? G");
Serial.println(code,DEC);
}
}
if (command_exists('M')) {
code = getValue('M');
switch(code) {
case 3: // turn on motor
case 4:
motor_on();
break;
case 5: // turn off motor
motor_off();
break;
case 82:
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
for (int i=0; i<20; i++) {
k=0;
PORTB |= _BV(5); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.println(k,DEC);
PORTB &= ~_BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(12.5*stepping);
}
}
break;
case 81:
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
while(1) {
if (PINC & _BV(0)) Serial.println("high");
else Serial.println("low");
}
break;
case 80: //plot out surface of milling area
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
fp.x = 0;
fp.y = 0;
fp.z = 0;
set_target(&fp);
set_position(&fp);
r_move(0);
for (int i=0; i<160; i+=2) {
for (float j=0; j<75; j+=2) {
fp.x=i;
fp.y=j;
fp.z=0;
set_target( &fp );
r_move( 0 );
k=0;
PORTB &= ~(_BV(5)); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.print(i,DEC);
Serial.print(",");
Serial.print(j,DEC);
Serial.print(",");
Serial.println(k,DEC);
PORTB |= _BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
}
}
break;
case 90: //plot out surface of milling area
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
// setup initial position
fp.x = 0;
fp.y = 135;
fp.z = 0;
set_target(&fp);
set_position(&fp);
r_move(0);
for (int i=0; i<1; i++) {
for (float j=135; j!=0; j-=.25) {
fp.x=i;
fp.y=j;
fp.z=0;
set_target( &fp );
r_move( 0 );
k=0;
PORTB |= _BV(5); //go down
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
k++;
}
//print result for this point
Serial.print(i,DEC);
Serial.print(",");
Serial.print(j,DEC);
Serial.print(",");
Serial.println(k,DEC);
PORTB &= ~_BV(5); //move up to origin
while (k--) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
}
}
break;
case 98: //M98 Find Z0 where it is one step from touching.
DDRC |= _BV(1);
PORTC &= ~_BV(1);
DDRC &= ~_BV(0);
PORTC |= _BV(0);
PORTB |= _BV(5);
while(PINC & _BV(0)) {
PORTB |= _BV(2);
delayMicroseconds(1);
PORTB &= ~_BV(2);
delayMicroseconds(200);
}
break;
case 99: //M99 S{1,2,4,8,16} -- set stepping mode
if (command_exists('S')) {
code = getValue('S');
if (code == 1 || code == 2 || code == 4 || code == 8 || code == 16) {
stepping = code;
setStep(stepping);
break;
}
}
default:
Serial.print("huh? M");
Serial.println(code,DEC);
}
}
Serial.println("ok");//tell our host we're done.
}
