summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authoradrian-bowyer <adrian-bowyer@cb376a5e-1013-0410-a455-b6b1f9ac8223>2009-10-27 15:34:16 +0000
committeradrian-bowyer <adrian-bowyer@cb376a5e-1013-0410-a455-b6b1f9ac8223>2009-10-27 15:34:16 +0000
commitbc9d4b965f993f00a2c1cf8882ee7fc833c905f7 (patch)
tree1b45f2b96c1c7ae2e8df316ac3de6fedf21a8a55
parent864d9365b7dd73ae25a4de570c78a73f05815f46 (diff)
downloadreprap-backup-bc9d4b965f993f00a2c1cf8882ee7fc833c905f7.tar.gz
reprap-backup-bc9d4b965f993f00a2c1cf8882ee7fc833c905f7.zip
Updating the Darwin 5D G code firmware.
git-svn-id: https://reprap.svn.sourceforge.net/svnroot/reprap@3328 cb376a5e-1013-0410-a455-b6b1f9ac8223
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/FiveD_GCode_Extruder.pde45
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/configuration.h32
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.h52
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.pde229
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.h87
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.pde106
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde353
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/ThermistorTable.h54
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.cpp1747
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.eep1
-rwxr-xr-xtrunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.elfbin82708 -> 0 bytes
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.hex1107
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.pde353
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/ThermistorTable.h54
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.h174
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.pde468
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/core.abin87666 -> 0 bytes
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.h230
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.pde308
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.h38
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.pde58
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/parameters.h106
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/pins.h142
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/process_g_code.pde525
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/vectors.h130
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h174
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h.dist174
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.pde468
-rwxr-xr-xtrunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/download-copy7
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h230
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h.dist230
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.pde308
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.h38
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.pde58
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h106
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h.dist105
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h142
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h.dist142
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/process_g_code.pde525
-rwxr-xr-xtrunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/upload-copy7
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h130
-rw-r--r--trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h.dist130
42 files changed, 0 insertions, 9373 deletions
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/FiveD_GCode_Extruder.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/FiveD_GCode_Extruder.pde
deleted file mode 100644
index 3e7bc487..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/FiveD_GCode_Extruder.pde
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * 5D GCode Interpreter
- * Arduino code to load into the extruder controller
- *
- * Adrian Bowyer 3 July 2009
- */
-
- /*
- ***NOTE***
- This program changes the frequency of Timer 0 so that PWM on pins H1E and H2E goes at
- a very high frequency (64kHz see:
- http://tzechienchu.typepad.com/tc_chus_point/2009/05/changing-pwm-frequency-on-the-arduino-diecimila.html)
-This will mess up timings in the delay() and similar functions; they will no longer work in
- milliseconds, but 64 times faster.
- */
-
-#ifndef __AVR_ATmega168__
-#error Oops! Make sure you have 'Arduino Diecimila' selected from the boards menu.
-#endif
-
-#include <ctype.h>
-#include <HardwareSerial.h>
-#include "WProgram.h"
-#include "configuration.h"
-#include "extruder.h"
-#include "intercom.h"
-
-intercom talker;
-extruder ex;
-
-void setup()
-{
- // All done in the class constructors
-}
-
-void loop()
-{
- // What should I do?
-
- talker.getAndProcessCommand();
-
- // Keep me at the right temp etc.
-
- ex.manage();
-}
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/configuration.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/configuration.h
deleted file mode 100644
index 4039110a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/configuration.h
+++ /dev/null
@@ -1,32 +0,0 @@
-
-
-#ifndef CONFIGURATION_H
-#define CONFIGURATION_H
-
-#define MY_NAME "TP" // Two byte string representing the name of this device
-
-// Communication speed
-
-#define RS485_BAUD 38400
-
-//******************************************************************************************************
-
-// Pin defintion section. This is for the RepRap Extruder Controler V2.2
-
-//our RS485 pins
-
-#define RX_ENABLE_PIN 4
-#define TX_ENABLE_PIN 16
-
-// Control pins for the A3949 chips
-
-#define H1D 7
-#define H1E 5
-#define H2D 8
-#define H2E 6
-
-// Analogue read of this pin gets the potentiometer setting
-
-#define POT 0
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.h
deleted file mode 100644
index 6c04efdb..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef EXTRUDER_H
-#define EXTRUDER_H
-
-#define WAIT_T 'W' // wait_for_temperature();
-#define VALVE 'V' // valve_set(bool open, int dTime);
-#define DIRECTION 'D' // set_direction(bool direction);
-#define COOL 'C' // set_cooler(byte e_speed);
-#define SET_T 'T' // set_temperature(int temp);
-#define GET_T 't' // get_temperature();
-#define STEP 'S' // step();
-#define ENABLE 'E' // enableStep();
-#define DISABLE 'e' // disableStep();
-#define PING 'P' // Just acknowledge
-
-class extruder
-{
-private:
-//********************************************************************************
-// Stepper motor section
-
-// We will half-step; coilPosition will take values between 0 and 7 inclusive
-
- byte coilPosition;
-
-// This variable stores the value (0..255) of the on-board potentiometer. This is used
-// to vary the PWM mark-space values in analogWrites() to the enable pins, hence
-// controlling the effective motor current.
-
- byte potValue;
- bool h1Enable;
- bool h2Enable;
- bool forward;
-
-
-public:
- extruder();
- void wait_for_temperature();
- void valve_set(bool open);
- void set_direction(bool direction);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
- void step();
- void enableStep();
- void disableStep();
- void processCommand(char command[]);
-
-};
-
-#endif
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.pde
deleted file mode 100644
index c0b68b3a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/extruder.pde
+++ /dev/null
@@ -1,229 +0,0 @@
-#include "extruder.h"
-
-extruder::extruder()
-{
- pinMode(H1D, OUTPUT);
- pinMode(H1E, OUTPUT);
- pinMode(H2D, OUTPUT);
- pinMode(H2E, OUTPUT);
- pinMode(POT, INPUT);
-
- // Change the frequency of Timer 0 so that PWM on pins H1E and H2E goes at
- // a very high frequency (64kHz see:
- // http://tzechienchu.typepad.com/tc_chus_point/2009/05/changing-pwm-frequency-on-the-arduino-diecimila.html)
-
- TCCR0B &= ~(0x07);
- TCCR0B |= 1;
-
- coilPosition = 0;
- forward = true;
-
-}
-
-void extruder::wait_for_temperature()
-{
-
-}
-
-void extruder::valve_set(bool open)
-{
-
-}
-
-void extruder::set_direction(bool direction)
-{
- forward = direction;
-}
-
-void extruder::set_cooler(byte e_speed)
-{
-
-}
-
-void extruder::set_temperature(int temp)
-{
-
-}
-
-int extruder::get_temperature()
-{
- return 1;
-}
-
-void extruder::manage()
-{
-
-}
-
-void extruder::step()
-{
-// This increments or decrements coilPosition then writes the appropriate pattern to the output pins.
-
- if(forward)
- coilPosition++;
- else
- coilPosition--;
- coilPosition &= 7;
-
- // Disable the coils
-
- digitalWrite(H1E, 0);
- digitalWrite(H2E, 0);
-
- // Which of the 8 possible patterns do we want?
- // The commented out setPower(...) lines could
- // be used to run the coils at constant power (with
- // half-stepping not each step is equal). Just
- // use the value of the argument to setPower(...) to
- // scale the PWM values.
-
- switch(coilPosition)
- {
- case 7:
- //setPower((stepPower >> 1) + (stepPower >> 3));
- digitalWrite(H1D, 1);
- digitalWrite(H2D, 1);
- h1Enable = true;
- h2Enable = true;
- break;
-
- case 6:
- //setPower(stepPower);
- digitalWrite(H1D, 1);
- digitalWrite(H2D, 1);
- h1Enable = true;
- h2Enable = false;
- break;
-
- case 5:
- //setPower((stepPower >> 1) + (stepPower >> 3));
- digitalWrite(H1D, 1);
- digitalWrite(H2D, 0);
- h1Enable = true;
- h2Enable = true;
- break;
-
- case 4:
- //setPower(stepPower);
- digitalWrite(H1D, 1);
- digitalWrite(H2D, 0);
- h1Enable = false;
- h2Enable = true;
- break;
-
- case 3:
- //setPower((stepPower >> 1) + (stepPower >> 3));
- digitalWrite(H1D, 0);
- digitalWrite(H2D, 0);
- h1Enable = true;
- h2Enable = true;
- break;
-
- case 2:
- //setPower(stepPower);
- digitalWrite(H1D, 0);
- digitalWrite(H2D, 0);
- h1Enable = true;
- h2Enable = false;
- break;
-
- case 1:
- //setPower((stepPower >> 1) + (stepPower >> 3));
- digitalWrite(H1D, 0);
- digitalWrite(H2D, 1);
- h1Enable = true;
- h2Enable = true;
- break;
-
- case 0:
- //setPower(stepPower);
- digitalWrite(H1D, 0);
- digitalWrite(H2D, 1);
- h1Enable = false;
- h2Enable = true;
- break;
-
- }
-
- // How much is the pot turned up?
- // Divide it by 4 to spread the valid readings out a bit.
- // This is about right for a 1A 3-ohm/coil stepper.
-
- potValue = analogRead(POT)>>2;
-
- // Send the appropriate PWM values
-
- if(h1Enable)
- analogWrite(H1E, potValue);
- else
- analogWrite(H1E, 0);
-
- if(h2Enable)
- analogWrite(H2E, potValue);
- else
- analogWrite(H2E, 0);
-}
-
-
-void extruder::enableStep()
-{
-// Nothing to do here - step() automatically enables the stepper drivers appropriately.
-}
-
-void extruder::disableStep()
-{
- analogWrite(H1E, 0);
- analogWrite(H2E, 0);
-}
-
-void extruder::processCommand(char command[])
-{
- switch(command[0])
- {
- case WAIT_T:
- wait_for_temperature();
- break;
-
- case VALVE:
- valve_set(command[1] == '1');
- break;
-
- case DIRECTION:
- set_direction(command[1] == '1');
- break;
-
- case COOL:
- set_cooler(atoi(&command[1]));
- break;
-
- case SET_T:
- set_temperature(atoi(&command[1]));
- break;
-
- case GET_T:
- itoa(get_temperature(), &(talker.getReply())[1], 10);
- break;
-
- case STEP:
- step();
- break;
-
- case ENABLE:
- enableStep();
- break;
-
- case DISABLE:
- disableStep();
- break;
-
- case PING:
- break;
-
- default:
- // Dud command, so it doesn't matter if we corrupt it -
- // pick out just the first char:
- command[1] = 0;
- talker.setReply(" !Dud command: ");
- talker.addReply(command);
- }
-}
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.h
deleted file mode 100644
index 40aa1edb..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
- /*
-
- A data packet looks like this:
-
- T T F F d a t a '\n'
-
- All these are printable characters. TT is the two-char name of the destination, FF is the two-char name
- of the source, data is a char string containing the message, which is terminated by a '\n' character.
- If users wish they can include checksum and length information in the data, as long as it is all printable.
- The total length of a packet with all of that should not exceed BUF_LEN (defined in configuration.h)
- characters. When a packet is received the '\n' character is replaced by 0, thus forming a standard C string.
-
- */
-
-#ifndef INTERCOM_H
-#define INTERCOM_H
-
-// Our RS485 driver
-
-#define serialInterface Serial
-
-// compare two names
-#define sameName(a, b) ((a)[0] == (b)[0] && (a)[1] == (b)[1])
-
-// The acknowledge character
-#define ACK 'A'
-
-// Size of the transmit and receive buffers
-#define BUF_LEN 30
-
-class intercom
-{
- private:
-
- // The input buffer for incoming packets
- char buffer[BUF_LEN];
- int buf_pointer;
-
-/*
- Any short message can be returned with the next acknowledgement in this string.
- This includes things like temperatures and so on.
-
- Assign to it like this:
-
- setReply(" My message");
-
- with a blank first character. This will be overwritten with the ACK
- character.
-
- Errors should be flagged with '!':
-
- setReply(" !Horrible error.");
-
- You can append extra information on the end with
-
- addReply("additional info");
-
- Don't exceed BUF_LEN - 4
-
-*/
- char reply[BUF_LEN];
- bool ok;
- void getPacket(char* string, int len);
- void sendPacket(char* to, char* string);
- void sendPacketWithReply(char* to, char* string, char* resp, int len);
- void reset();
- void acknowledgeAndReset(char* to);
-
- public:
- intercom();
- void getAndProcessCommand();
- void setReply(char* string);
- void addReply(char* string);
- char* getReply();
-};
-
-extern intercom talker;
-
-#endif
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.pde
deleted file mode 100644
index 8c775a94..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Extruder/intercom.pde
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#include "intercom.h"
-
-intercom::intercom()
-{
- reset();
- pinMode(RX_ENABLE_PIN, OUTPUT);
- pinMode(TX_ENABLE_PIN, OUTPUT);
- digitalWrite(RX_ENABLE_PIN, 0); //always listen.
- serialInterface.begin(RS485_BAUD);
-}
-
-void intercom::sendPacket(char* to, char* string)
-{
- digitalWrite(TX_ENABLE_PIN, 1);
- serialInterface.print(to[0]); // Make sure we send exactly two bytes
- serialInterface.print(to[1]);
- serialInterface.print(MY_NAME); // This should be two bytes
- serialInterface.println(string);
- digitalWrite(TX_ENABLE_PIN, 0);
-}
-
-void intercom::sendPacketWithReply(char* to, char* string, char* resp, int len)
-{
- sendPacket(to, string);
- getPacket(resp, len);
-}
-
-void intercom::getPacket(char* string, int len)
-{
- int i = -1;
- ok = true;
- do
- {
- while(!serialInterface.available());
- i++;
- // Stop runaway buffer overflow
- if(i >= len)
- {
- ok = false;
- i--;
- } else
- string[i] = serialInterface.read();
- } while(string[i] != '\n' && ok);
- string[i] = 0;
-}
-
-void intercom::reset()
-{
- buf_pointer = 0;
- reply[0] = 0;
-}
-
-void intercom::acknowledgeAndReset(char* to)
-{
- if(!reply[0])
- reply[1] = 0;
- reply[0] = ACK;
- talker.sendPacket(to, reply);
- reset();
-}
-
-void intercom::getAndProcessCommand()
-{
- if(!serialInterface.available())
- return;
- buffer[buf_pointer] = serialInterface.read();
- if(buffer[buf_pointer] == '\n')
- {
- buffer[buf_pointer] = 0;
- if(!sameName(buffer, MY_NAME)) // For me?
- {
- reset(); // No
- return;
- }
- ex.processCommand(&buffer[4]);
- acknowledgeAndReset(&buffer[2]);
- }else
- buf_pointer++;
- if(buf_pointer >= BUF_LEN)
- {
- reset();
- setReply(" !Buffer overrun");
- }
-}
-
-void intercom::setReply(char* string)
-{
- strcpy(reply, string);
-}
-
-void intercom::addReply(char* string)
-{
- strcat(reply, string);
-}
-
-char* intercom::getReply()
-{
- return reply;
-}
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde
deleted file mode 100644
index 15b5311d..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde
+++ /dev/null
@@ -1,353 +0,0 @@
-// Yep, this is actually -*- c++ -*-
-
-// Sanguino G-code Interpreter
-// Arduino v1.0 by Mike Ellery - initial software (mellery@gmail.com)
-// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)
-// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)
-// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
-// Sanguino v1.4 by Adrian Bowyer - added the Sanguino; extensive mods... (a.bowyer@bath.ac.uk)
-// Sanguino v1.5 by Adrian Bowyer - implemented 4D Bressenham XYZ+ stepper control... (a.bowyer@bath.ac.uk)
-// Sanguino v1.6 by Adrian Bowyer - implemented RS485 extruders
-
-#ifndef __AVR_ATmega644P__
-#error Oops! Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu.
-#endif
-
-#include <ctype.h>
-#include <HardwareSerial.h>
-#include "WProgram.h"
-#include "vectors.h"
-#include "parameters.h"
-#include "intercom.h"
-#include "pins.h"
-#include "extruder.h"
-#include "cartesian_dda.h"
-
-// Maintain a list of extruders...
-
-extruder* ex[EXTRUDER_COUNT];
-byte extruder_in_use = 0;
-
-// Text placed in this (terminated with 0) will be transmitted back to the host
-// along with the next G Code acknowledgement.
-char debugstring[10];
-
-#if MOTHERBOARD < 2
-
-// TODO: For some reason, if you declare the following two in the order ex0 ex1 then
-// ex0 won't drive its stepper. They seem fine this way round though. But that's got
-// to be a bug.
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(EXTRUDER_1_MOTOR_DIR_PIN, EXTRUDER_1_MOTOR_SPEED_PIN , EXTRUDER_1_HEATER_PIN,
- EXTRUDER_1_FAN_PIN, EXTRUDER_1_TEMPERATURE_PIN, EXTRUDER_1_VALVE_DIR_PIN,
- EXTRUDER_1_VALVE_ENABLE_PIN, EXTRUDER_1_STEP_ENABLE_PIN);
-#endif
-
-static extruder ex0(EXTRUDER_0_MOTOR_DIR_PIN, EXTRUDER_0_MOTOR_SPEED_PIN , EXTRUDER_0_HEATER_PIN,
- EXTRUDER_0_FAN_PIN, EXTRUDER_0_TEMPERATURE_PIN, EXTRUDER_0_VALVE_DIR_PIN,
- EXTRUDER_0_VALVE_ENABLE_PIN, EXTRUDER_0_STEP_ENABLE_PIN);
-
-
-#else
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(2);
-#endif
-
-static extruder ex0(1);
-
-intercom talker;
-
-#endif
-
-// Each entry in the buffer is an instance of cartesian_dda.
-
-cartesian_dda* cdda[BUFFER_SIZE];
-
-static cartesian_dda cdda0;
-static cartesian_dda cdda1;
-static cartesian_dda cdda2;
-static cartesian_dda cdda3;
-
-volatile byte head;
-volatile byte tail;
-
-// Where the machine is from the point of view of the command stream
-
-FloatPoint where_i_am;
-
-// Make sure each DDA knows which extruder to use
-
-//inline void setExtruder()
-//{
-// for(byte i = 0; i < BUFFER_SIZE; i++)
-// cdda[i]->set_extruder(ex[extruder_in_use]);
-//}
-
-
-// Our interrupt function
-
-SIGNAL(SIG_OUTPUT_COMPARE1A)
-{
- disableTimerInterrupt();
-
- if(cdda[tail]->active())
- cdda[tail]->dda_step();
- else
- dQMove();
-
- enableTimerInterrupt();
-}
-
-void setup()
-{
- disableTimerInterrupt();
- setupTimerInterrupt();
- debugstring[0] = 0;
-
- ex[0] = &ex0;
-#if EXTRUDER_COUNT == 2
- ex[1] = &ex1;
-#endif
- extruder_in_use = 0;
-
- head = 0;
- tail = 0;
-
- cdda[0] = &cdda0;
- cdda[1] = &cdda1;
- cdda[2] = &cdda2;
- cdda[3] = &cdda3;
-
- //setExtruder();
-
- init_process_string();
-
- where_i_am.x = 0.0;
- where_i_am.y = 0.0;
- where_i_am.z = 0.0;
- where_i_am.e = 0.0;
- where_i_am.f = SLOW_XY_FEEDRATE;
-
- Serial.begin(19200);
- Serial.println("start");
-
- setTimer(DEFAULT_TICK);
- enableTimerInterrupt();
-}
-
-void loop()
-{
- manage_all_extruders();
- get_and_do_command();
-}
-
-//******************************************************************************************
-
-// The move buffer
-
-inline bool qFull()
-{
- if(tail == 0)
- return head == (BUFFER_SIZE - 1);
- else
- return head == (tail - 1);
-}
-
-inline bool qEmpty()
-{
- return tail == head && !cdda[tail]->active();
-}
-
-inline void qMove(const FloatPoint& p)
-{
- while(qFull()) delay(WAITING_DELAY);
- byte h = head;
- h++;
- if(h >= BUFFER_SIZE)
- h = 0;
- cdda[h]->set_target(p);
- head = h;
-}
-
-inline void dQMove()
-{
- if(qEmpty())
- return;
- byte t = tail;
- t++;
- if(t >= BUFFER_SIZE)
- t = 0;
- cdda[t]->dda_start();
- tail = t;
-}
-
-inline void setUnits(bool u)
-{
- for(byte i = 0; i < BUFFER_SIZE; i++)
- cdda[i]->set_units(u);
-}
-
-
-inline void setPosition(const FloatPoint& p)
-{
- where_i_am = p;
-}
-
-
-//******************************************************************************************
-
-// Interrupt functions
-
-void setupTimerInterrupt()
-{
- //clear the registers
- TCCR1A = 0;
- TCCR1B = 0;
- TCCR1C = 0;
- TIMSK1 = 0;
-
- //waveform generation = 0100 = CTC
- TCCR1B &= ~(1<<WGM13);
- TCCR1B |= (1<<WGM12);
- TCCR1A &= ~(1<<WGM11);
- TCCR1A &= ~(1<<WGM10);
-
- //output mode = 00 (disconnected)
- TCCR1A &= ~(1<<COM1A1);
- TCCR1A &= ~(1<<COM1A0);
- TCCR1A &= ~(1<<COM1B1);
- TCCR1A &= ~(1<<COM1B0);
-
- //start off with a slow frequency.
- setTimerResolution(4);
- setTimerCeiling(65535);
-}
-
-void setTimerResolution(byte r)
-{
- //here's how you figure out the tick size:
- // 1000000 / ((16000000 / prescaler))
- // 1000000 = microseconds in 1 second
- // 16000000 = cycles in 1 second
- // prescaler = your prescaler
-
- // no prescaler == 0.0625 usec tick
- if (r == 0)
- {
- // 001 = clk/1
- TCCR1B &= ~(1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /8 == 0.5 usec tick
- else if (r == 1)
- {
- // 010 = clk/8
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /64 == 4 usec tick
- else if (r == 2)
- {
- // 011 = clk/64
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /256 == 16 usec tick
- else if (r == 3)
- {
- // 100 = clk/256
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /1024 == 64 usec tick
- else
- {
- // 101 = clk/1024
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
-}
-
-unsigned int getTimerCeiling(const long& delay)
-{
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs)
- if (delay <= 65535L)
- return (delay & 0xffff);
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs)
- else if (delay <= 524280L)
- return ((delay / 8) & 0xffff);
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs)
- else if (delay <= 4194240L)
- return ((delay / 64) & 0xffff);
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs)
- else if (delay <= 16776960L)
- return ((delay / 256) & 0xffff);
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs)
- else if (delay <= 67107840L)
- return ((delay / 1024) & 0xffff);
- //its really slow... hopefully we can just get by with super slow.
- else
- return 65535;
-}
-
-byte getTimerResolution(const long& delay)
-{
- // these also represent frequency: 1000000 / delay / 2 = frequency in hz.
-
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs (4 millisecond max))
- // range: 8Mhz max - 122hz min
- if (delay <= 65535L)
- return 0;
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs (32 millisecond max))
- // range:1Mhz max - 15.26hz min
- else if (delay <= 524280L)
- return 1;
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs (0.26 seconds max))
- // range: 125Khz max - 1.9hz min
- else if (delay <= 4194240L)
- return 2;
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs (1.04 seconds max))
- // range: 31.25Khz max - 0.475hz min
- else if (delay <= 16776960L)
- return 3;
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max))
- // range: 7.812Khz max - 0.119hz min
- else if (delay <= 67107840L)
- return 4;
- //its really slow... hopefully we can just get by with super slow.
- else
- return 4;
-}
-
-
-// Depending on how much work the interrupt function has to do, this is
-// pretty accurate between 10 us and 0.1 s. At fast speeds, the time
-// taken in the interrupt function becomes significant, of course.
-
-// Note - it is up to the user to call enableTimerInterrupt() after a call
-// to this function.
-
-inline void setTimer(long delay)
-{
- // delay is the delay between steps in microsecond ticks.
- //
- // we break it into 5 different resolutions based on the delay.
- // then we set the resolution based on the size of the delay.
- // we also then calculate the timer ceiling required. (ie what the counter counts to)
- // the result is the timer counts up to the appropriate time and then fires an interrupt.
-
- // Actual ticks are 0.0625 us, so multiply delay by 16
-
- delay <<= 4;
-
- setTimerCeiling(getTimerCeiling(delay));
- setTimerResolution(getTimerResolution(delay));
-}
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/ThermistorTable.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/ThermistorTable.h
deleted file mode 100644
index c55b9c91..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/ThermistorTable.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef THERMISTORTABLE_H_
-#define THERMISTORTABLE_H_
-
-#if MOTHERBOARD < 2
-
-// Uncomment the next line if you are using a thermistor; leave it if you have a thermocouple
-//#define USE_THERMISTOR
-
-// How many temperature samples to take for an average. each sample takes about 100 usecs.
-#define TEMPERATURE_SAMPLES 3
-
-// How accurately do we maintain the temperature?
-#define HALF_DEAD_ZONE 5
-
-// Thermistor lookup table for RepRap Temperature Sensor Boards (http://make.rrrf.org/ts)
-// See this page:
-// http://dev.www.reprap.org/bin/view/Main/Thermistor
-// for details of what goes in this table.
-// Made with createTemperatureLookup.py (http://svn.reprap.org/trunk/reprap/firmware/Arduino/utilities/createTemperatureLookup.py)
-// ./createTemperatureLookup.py --r0=100000 --t0=25 --r1=0 --r2=4700 --beta=4066 --max-adc=1023
-// r0: 100000
-// t0: 25
-// r1: 0
-// r2: 4700
-// beta: 4066
-// max adc: 1023
-#ifdef USE_THERMISTOR
-#define NUMTEMPS 20
-short temptable[NUMTEMPS][2] = {
- {1, 841},
- {54, 255},
- {107, 209},
- {160, 184},
- {213, 166},
- {266, 153},
- {319, 142},
- {372, 132},
- {425, 124},
- {478, 116},
- {531, 108},
- {584, 101},
- {637, 93},
- {690, 86},
- {743, 78},
- {796, 70},
- {849, 61},
- {902, 50},
- {955, 34},
- {1008, 3}
-};
-
-#endif
-#endif
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.cpp b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.cpp
deleted file mode 100644
index 41616e19..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.cpp
+++ /dev/null
@@ -1,1747 +0,0 @@
-// Yep, this is actually -*- c++ -*-
-
-// Sanguino G-code Interpreter
-// Arduino v1.0 by Mike Ellery - initial software (mellery@gmail.com)
-// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)
-// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)
-// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
-// Sanguino v1.4 by Adrian Bowyer - added the Sanguino; extensive mods... (a.bowyer@bath.ac.uk)
-// Sanguino v1.5 by Adrian Bowyer - implemented 4D Bressenham XYZ+ stepper control... (a.bowyer@bath.ac.uk)
-// Sanguino v1.6 by Adrian Bowyer - implemented RS485 extruders
-
-#ifndef __AVR_ATmega644P__
-#error Oops! Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu.
-#endif
-
-#include <ctype.h>
-#include <HardwareSerial.h>
-#include "WProgram.h"
-#include "vectors.h"
-#include "parameters.h"
-#include "intercom.h"
-#include "pins.h"
-#include "extruder.h"
-#include "cartesian_dda.h"
-
-// Maintain a list of extruders...
-
-#include "WProgram.h"
-void setup();
-void loop();
-inline bool qFull();
-inline bool qEmpty();
-inline void dQMove();
-inline void setUnits(bool u);
-void setupTimerInterrupt();
-void setTimerResolution(byte r);
-inline void setTimer(long delay);
-void manage_all_extruders();
-void new_extruder(byte e);
-inline void init_process_string();
-void get_and_do_command();
-int parse_string(struct GcodeParser * gc, char instruction[ ], int size);
-void process_string(char instruction[], int size);
-int scan_float(char *str, float *valp, unsigned int *seen, unsigned int flag);
-int scan_int(char *str, int *valp, unsigned int *seen, unsigned int flag);
-extruder* ex[EXTRUDER_COUNT];
-byte extruder_in_use = 0;
-
-// Text placed in this (terminated with 0) will be transmitted back to the host
-// along with the next G Code acknowledgement.
-char debugstring[10];
-
-#if MOTHERBOARD < 2
-
-// TODO: For some reason, if you declare the following two in the order ex0 ex1 then
-// ex0 won't drive its stepper. They seem fine this way round though. But that's got
-// to be a bug.
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(EXTRUDER_1_MOTOR_DIR_PIN, EXTRUDER_1_MOTOR_SPEED_PIN , EXTRUDER_1_HEATER_PIN,
- EXTRUDER_1_FAN_PIN, EXTRUDER_1_TEMPERATURE_PIN, EXTRUDER_1_VALVE_DIR_PIN,
- EXTRUDER_1_VALVE_ENABLE_PIN, EXTRUDER_1_STEP_ENABLE_PIN);
-#endif
-
-static extruder ex0(EXTRUDER_0_MOTOR_DIR_PIN, EXTRUDER_0_MOTOR_SPEED_PIN , EXTRUDER_0_HEATER_PIN,
- EXTRUDER_0_FAN_PIN, EXTRUDER_0_TEMPERATURE_PIN, EXTRUDER_0_VALVE_DIR_PIN,
- EXTRUDER_0_VALVE_ENABLE_PIN, EXTRUDER_0_STEP_ENABLE_PIN);
-
-
-#else
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(2);
-#endif
-
-static extruder ex0(1);
-
-intercom talker;
-
-#endif
-
-// Each entry in the buffer is an instance of cartesian_dda.
-
-cartesian_dda* cdda[BUFFER_SIZE];
-
-static cartesian_dda cdda0;
-static cartesian_dda cdda1;
-static cartesian_dda cdda2;
-static cartesian_dda cdda3;
-
-volatile byte head;
-volatile byte tail;
-
-// Where the machine is from the point of view of the command stream
-
-FloatPoint where_i_am;
-
-// Make sure each DDA knows which extruder to use
-
-//inline void setExtruder()
-//{
-// for(byte i = 0; i < BUFFER_SIZE; i++)
-// cdda[i]->set_extruder(ex[extruder_in_use]);
-//}
-
-
-// Our interrupt function
-
-SIGNAL(SIG_OUTPUT_COMPARE1A)
-{
- disableTimerInterrupt();
-
- if(cdda[tail]->active())
- cdda[tail]->dda_step();
- else
- dQMove();
-
- enableTimerInterrupt();
-}
-
-void setup()
-{
- disableTimerInterrupt();
- setupTimerInterrupt();
- debugstring[0] = 0;
-
- ex[0] = &ex0;
-#if EXTRUDER_COUNT == 2
- ex[1] = &ex1;
-#endif
- extruder_in_use = 0;
-
- head = 0;
- tail = 0;
-
- cdda[0] = &cdda0;
- cdda[1] = &cdda1;
- cdda[2] = &cdda2;
- cdda[3] = &cdda3;
-
- //setExtruder();
-
- init_process_string();
-
- where_i_am.x = 0.0;
- where_i_am.y = 0.0;
- where_i_am.z = 0.0;
- where_i_am.e = 0.0;
- where_i_am.f = SLOW_XY_FEEDRATE;
-
- Serial.begin(19200);
- Serial.println("start");
-
- setTimer(DEFAULT_TICK);
- enableTimerInterrupt();
-}
-
-void loop()
-{
- manage_all_extruders();
- get_and_do_command();
-}
-
-//******************************************************************************************
-
-// The move buffer
-
-inline bool qFull()
-{
- if(tail == 0)
- return head == (BUFFER_SIZE - 1);
- else
- return head == (tail - 1);
-}
-
-inline bool qEmpty()
-{
- return tail == head && !cdda[tail]->active();
-}
-
-inline void qMove(const FloatPoint& p)
-{
- while(qFull()) delay(WAITING_DELAY);
- byte h = head;
- h++;
- if(h >= BUFFER_SIZE)
- h = 0;
- cdda[h]->set_target(p);
- head = h;
-}
-
-inline void dQMove()
-{
- if(qEmpty())
- return;
- byte t = tail;
- t++;
- if(t >= BUFFER_SIZE)
- t = 0;
- cdda[t]->dda_start();
- tail = t;
-}
-
-inline void setUnits(bool u)
-{
- for(byte i = 0; i < BUFFER_SIZE; i++)
- cdda[i]->set_units(u);
-}
-
-
-inline void setPosition(const FloatPoint& p)
-{
- where_i_am = p;
-}
-
-
-//******************************************************************************************
-
-// Interrupt functions
-
-void setupTimerInterrupt()
-{
- //clear the registers
- TCCR1A = 0;
- TCCR1B = 0;
- TCCR1C = 0;
- TIMSK1 = 0;
-
- //waveform generation = 0100 = CTC
- TCCR1B &= ~(1<<WGM13);
- TCCR1B |= (1<<WGM12);
- TCCR1A &= ~(1<<WGM11);
- TCCR1A &= ~(1<<WGM10);
-
- //output mode = 00 (disconnected)
- TCCR1A &= ~(1<<COM1A1);
- TCCR1A &= ~(1<<COM1A0);
- TCCR1A &= ~(1<<COM1B1);
- TCCR1A &= ~(1<<COM1B0);
-
- //start off with a slow frequency.
- setTimerResolution(4);
- setTimerCeiling(65535);
-}
-
-void setTimerResolution(byte r)
-{
- //here's how you figure out the tick size:
- // 1000000 / ((16000000 / prescaler))
- // 1000000 = microseconds in 1 second
- // 16000000 = cycles in 1 second
- // prescaler = your prescaler
-
- // no prescaler == 0.0625 usec tick
- if (r == 0)
- {
- // 001 = clk/1
- TCCR1B &= ~(1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /8 == 0.5 usec tick
- else if (r == 1)
- {
- // 010 = clk/8
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /64 == 4 usec tick
- else if (r == 2)
- {
- // 011 = clk/64
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /256 == 16 usec tick
- else if (r == 3)
- {
- // 100 = clk/256
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /1024 == 64 usec tick
- else
- {
- // 101 = clk/1024
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
-}
-
-unsigned int getTimerCeiling(const long& delay)
-{
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs)
- if (delay <= 65535L)
- return (delay & 0xffff);
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs)
- else if (delay <= 524280L)
- return ((delay / 8) & 0xffff);
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs)
- else if (delay <= 4194240L)
- return ((delay / 64) & 0xffff);
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs)
- else if (delay <= 16776960L)
- return ((delay / 256) & 0xffff);
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs)
- else if (delay <= 67107840L)
- return ((delay / 1024) & 0xffff);
- //its really slow... hopefully we can just get by with super slow.
- else
- return 65535;
-}
-
-byte getTimerResolution(const long& delay)
-{
- // these also represent frequency: 1000000 / delay / 2 = frequency in hz.
-
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs (4 millisecond max))
- // range: 8Mhz max - 122hz min
- if (delay <= 65535L)
- return 0;
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs (32 millisecond max))
- // range:1Mhz max - 15.26hz min
- else if (delay <= 524280L)
- return 1;
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs (0.26 seconds max))
- // range: 125Khz max - 1.9hz min
- else if (delay <= 4194240L)
- return 2;
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs (1.04 seconds max))
- // range: 31.25Khz max - 0.475hz min
- else if (delay <= 16776960L)
- return 3;
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max))
- // range: 7.812Khz max - 0.119hz min
- else if (delay <= 67107840L)
- return 4;
- //its really slow... hopefully we can just get by with super slow.
- else
- return 4;
-}
-
-
-// Depending on how much work the interrupt function has to do, this is
-// pretty accurate between 10 us and 0.1 s. At fast speeds, the time
-// taken in the interrupt function becomes significant, of course.
-
-// Note - it is up to the user to call enableTimerInterrupt() after a call
-// to this function.
-
-inline void setTimer(long delay)
-{
- // delay is the delay between steps in microsecond ticks.
- //
- // we break it into 5 different resolutions based on the delay.
- // then we set the resolution based on the size of the delay.
- // we also then calculate the timer ceiling required. (ie what the counter counts to)
- // the result is the timer counts up to the appropriate time and then fires an interrupt.
-
- // Actual ticks are 0.0625 us, so multiply delay by 16
-
- delay <<= 4;
-
- setTimerCeiling(getTimerCeiling(delay));
- setTimerResolution(getTimerResolution(delay));
-}
-
-
-#include <stdio.h>
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-
-// Initialise X, Y and Z. The extruder is initialized
-// separately.
-
-cartesian_dda::cartesian_dda()
-{
- live = false;
- nullmove = false;
-
- // Default is going forward
-
- x_direction = 1;
- y_direction = 1;
- z_direction = 1;
- e_direction = 1;
- f_direction = 1;
-
- // Default to the origin and not going anywhere
-
- target_position.x = 0.0;
- target_position.y = 0.0;
- target_position.z = 0.0;
- target_position.e = 0.0;
- target_position.f = SLOW_XY_FEEDRATE;
-
- // Set up the pin directions
-
- pinMode(X_STEP_PIN, OUTPUT);
- pinMode(X_DIR_PIN, OUTPUT);
-
- pinMode(Y_STEP_PIN, OUTPUT);
- pinMode(Y_DIR_PIN, OUTPUT);
-
- pinMode(Z_STEP_PIN, OUTPUT);
- pinMode(Z_DIR_PIN, OUTPUT);
-
-#if MOTHERBOARD > 0
- pinMode(X_ENABLE_PIN, OUTPUT);
- pinMode(Y_ENABLE_PIN, OUTPUT);
- pinMode(Z_ENABLE_PIN, OUTPUT);
-#endif
-
- //turn the motors off at the start.
-
- disable_steppers();
-
-#if ENDSTOPS_MIN_ENABLED == 1
- pinMode(X_MIN_PIN, INPUT);
- pinMode(Y_MIN_PIN, INPUT);
- pinMode(Z_MIN_PIN, INPUT);
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
- pinMode(X_MAX_PIN, INPUT);
- pinMode(Y_MAX_PIN, INPUT);
- pinMode(Z_MAX_PIN, INPUT);
-#endif
-
- // Default units are mm
-
- set_units(true);
-}
-
-// Switch between mm and inches
-
-void cartesian_dda::set_units(bool using_mm)
-{
- if(using_mm)
- {
- units.x = X_STEPS_PER_MM;
- units.y = Y_STEPS_PER_MM;
- units.z = Z_STEPS_PER_MM;
- units.e = E_STEPS_PER_MM;
- units.f = 1.0;
- } else
- {
- units.x = X_STEPS_PER_INCH;
- units.y = Y_STEPS_PER_INCH;
- units.z = Z_STEPS_PER_INCH;
- units.e = E_STEPS_PER_INCH;
- units.f = 1.0;
- }
-}
-
-
-void cartesian_dda::set_target(const FloatPoint& p)
-{
- target_position = p;
- nullmove = false;
-
- //figure our deltas.
-
- delta_position = fabsv(target_position - where_i_am);
-
- // The feedrate values refer to distance in (X, Y, Z) space, so ignore e and f
- // values unless they're the only thing there.
-
- FloatPoint squares = delta_position*delta_position;
- distance = squares.x + squares.y + squares.z;
- // If we are 0, only thing changing is e
- if(distance <= 0.0)
- distance = squares.e;
- // If we are still 0, only thing changing is f
- if(distance <= 0.0)
- distance = squares.f;
- distance = sqrt(distance);
-
- //set our steps current, target, and delta
-
- current_steps = to_steps(units, where_i_am);
- target_steps = to_steps(units, target_position);
- delta_steps = absv(target_steps - current_steps);
-
- // find the dominant axis.
- // NB we ignore the f values here, as it takes no time to take a step in time :-)
-
- total_steps = max(delta_steps.x, delta_steps.y);
- total_steps = max(total_steps, delta_steps.z);
- total_steps = max(total_steps, delta_steps.e);
-
- // If we're not going anywhere, flag the fact
-
- if(total_steps == 0)
- {
- nullmove = true;
- where_i_am = p;
- return;
- }
-
-#ifndef ACCELERATION_ON
- current_steps.f = round(target_position.f);
-#endif
-
- delta_steps.f = abs(target_steps.f - current_steps.f);
-
- // Rescale the feedrate so it doesn't take lots of steps to do
-
- t_scale = 1;
- if(delta_steps.f > total_steps)
- {
- t_scale = delta_steps.f/total_steps;
- if(t_scale >= 3)
- {
- target_steps.f = target_steps.f/t_scale;
- current_steps.f = current_steps.f/t_scale;
- delta_steps.f = abs(target_steps.f - current_steps.f);
- if(delta_steps.f > total_steps)
- total_steps = delta_steps.f;
- } else
- {
- t_scale = 1;
- total_steps = delta_steps.f;
- }
- }
-
- //what is our direction?
-
- x_direction = (target_position.x >= where_i_am.x);
- y_direction = (target_position.y >= where_i_am.y);
- z_direction = (target_position.z >= where_i_am.z);
- e_direction = (target_position.e >= where_i_am.e);
- f_direction = (target_position.f >= where_i_am.f);
-
- dda_counter.x = -total_steps/2;
- dda_counter.y = dda_counter.x;
- dda_counter.z = dda_counter.x;
- dda_counter.e = dda_counter.x;
- dda_counter.f = dda_counter.x;
-
- where_i_am = p;
-
- return;
-}
-
-
-
-void cartesian_dda::dda_step()
-{
- if(!live)
- return;
-
- do
- {
- x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
- y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
- z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
- e_can_step = can_step(-1, -1, current_steps.e, target_steps.e, e_direction);
- f_can_step = can_step(-1, -1, current_steps.f, target_steps.f, f_direction);
-
- real_move = false;
-
- if (x_can_step)
- {
- dda_counter.x += delta_steps.x;
-
- if (dda_counter.x > 0)
- {
- do_x_step();
- real_move = true;
- dda_counter.x -= total_steps;
-
- if (x_direction)
- current_steps.x++;
- else
- current_steps.x--;
- }
- }
-
- if (y_can_step)
- {
- dda_counter.y += delta_steps.y;
-
- if (dda_counter.y > 0)
- {
- do_y_step();
- real_move = true;
- dda_counter.y -= total_steps;
-
- if (y_direction)
- current_steps.y++;
- else
- current_steps.y--;
- }
- }
-
- if (z_can_step)
- {
- dda_counter.z += delta_steps.z;
-
- if (dda_counter.z > 0)
- {
- do_z_step();
- real_move = true;
- dda_counter.z -= total_steps;
-
- if (z_direction)
- current_steps.z++;
- else
- current_steps.z--;
- }
- }
-
- if (e_can_step)
- {
- dda_counter.e += delta_steps.e;
-
- if (dda_counter.e > 0)
- {
- do_e_step();
- real_move = true;
- dda_counter.e -= total_steps;
-
- if (e_direction)
- current_steps.e++;
- else
- current_steps.e--;
- }
- }
-
- if (f_can_step)
- {
- dda_counter.f += delta_steps.f;
-
- if (dda_counter.f > 0)
- {
- dda_counter.f -= total_steps;
- if (f_direction)
- current_steps.f++;
- else
- current_steps.f--;
- }
- }
-
-
- // wait for next step.
- // Use milli- or micro-seconds, as appropriate
- // If the only thing that changed was f keep looping
-
- if(real_move)
- {
- if(t_scale > 1)
- timestep = t_scale*current_steps.f;
- else
- timestep = current_steps.f;
- timestep = calculate_feedrate_delay((float) timestep);
- setTimer(timestep);
- }
- } while (!real_move && f_can_step);
-
- live = (x_can_step || y_can_step || z_can_step || e_can_step || f_can_step);
-
-// Wrap up at the end of a line
-
- if(!live)
- {
- disable_steppers();
- setTimer(DEFAULT_TICK);
- }
-
-}
-
-
-// Run the DDA
-
-void cartesian_dda::dda_start()
-{
- // Set up the DDA
- //sprintf(debugstring, "%d %d", x_direction, nullmove);
-
- if(nullmove)
- return;
-
- //set our direction pins as well
-#if INVERT_X_DIR == 1
- digitalWrite(X_DIR_PIN, !x_direction);
-#else
- digitalWrite(X_DIR_PIN, x_direction);
-#endif
-
-#if INVERT_Y_DIR == 1
- digitalWrite(Y_DIR_PIN, !y_direction);
-#else
- digitalWrite(Y_DIR_PIN, y_direction);
-#endif
-
-#if INVERT_Z_DIR == 1
- digitalWrite(Z_DIR_PIN, !z_direction);
-#else
- digitalWrite(Z_DIR_PIN, z_direction);
-#endif
- if(e_direction)
- ex[extruder_in_use]->set_direction(1);
- else
- ex[extruder_in_use]->set_direction(0);
-
- //turn on steppers to start moving =)
-
- enable_steppers();
-
- // extcount = 0;
-
- setTimer(DEFAULT_TICK);
- live = true;
-}
-
-
-bool cartesian_dda::can_step(byte min_pin, byte max_pin, long current, long target, byte dir)
-{
-
- //stop us if we're on target
-
- if (target == current)
- return false;
-
-#if ENDSTOPS_MIN_ENABLED == 1
-
- //stop us if we're home and still going
-
- else if(min_pin >= 0)
- {
- if (read_switch(min_pin) && !dir)
- return false;
- }
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
-
- //stop us if we're at max and still going
-
- else if(max_pin >= 0)
- {
- if (read_switch(max_pin) && dir)
- return false;
- }
-#endif
-
- // All OK - we can step
-
- return true;
-}
-
-
-void cartesian_dda::enable_steppers()
-{
-#if MOTHERBOARD > 0
- if(delta_steps.x)
- digitalWrite(X_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.y)
- digitalWrite(Y_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.z)
- digitalWrite(Z_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.e)
- ex[extruder_in_use]->enableStep();
-#endif
-}
-
-
-
-void cartesian_dda::disable_steppers()
-{
-#if MOTHERBOARD > 0
- //disable our steppers
- digitalWrite(X_ENABLE_PIN, !ENABLE_ON);
- digitalWrite(Y_ENABLE_PIN, !ENABLE_ON);
- //digitalWrite(Z_ENABLE_PIN, !ENABLE_ON);
-
- // Disabling the extrude stepper causes the backpressure to
- // turn the motor the wrong way. Leave it on.
-
- //ex[extruder_in_use]->->disableStep();
-#endif
-}
-
-/*
-
-void cartesian_dda::delayMicrosecondsInterruptible(unsigned int us)
-{
-
-#if F_CPU >= 16000000L
- // for the 16 MHz clock on most Arduino boards
-
- // for a one-microsecond delay, simply return. the overhead
- // of the function call yields a delay of approximately 1 1/8 us.
- if (--us == 0)
- return;
-
- // the following loop takes a quarter of a microsecond (4 cycles)
- // per iteration, so execute it four times for each microsecond of
- // delay requested.
- us <<= 2;
-
- // account for the time taken in the preceeding commands.
- us -= 2;
-#else
- // for the 8 MHz internal clock on the ATmega168
-
- // for a one- or two-microsecond delay, simply return. the overhead of
- // the function calls takes more than two microseconds. can't just
- // subtract two, since us is unsigned; we'd overflow.
- if (--us == 0)
- return;
- if (--us == 0)
- return;
-
- // the following loop takes half of a microsecond (4 cycles)
- // per iteration, so execute it twice for each microsecond of
- // delay requested.
- us <<= 1;
-
- // partially compensate for the time taken by the preceeding commands.
- // we can't subtract any more than this or we'd overflow w/ small delays.
- us--;
-#endif
-
- // busy wait
- __asm__ __volatile__ (
- "1: sbiw %0,1" "\n\t" // 2 cycles
- "brne 1b" : "=w" (us) : "0" (us) // 2 cycles
- );
-}
-*/
-
-
-#include "parameters.h"
-#include "pins.h"
-#include "ThermistorTable.h"
-#include "extruder.h"
-
-// Keep all extruders up to temperature etc.
-
-void manage_all_extruders()
-{
- for(byte i = 0; i < EXTRUDER_COUNT; i++)
- ex[i]->manage();
-}
-
-// Select a new extruder
-
-void new_extruder(byte e)
-{
- if(e < 0)
- e = 0;
- if(e >= EXTRUDER_COUNT)
- e = EXTRUDER_COUNT - 1;
-
- if(e != extruder_in_use)
- {
- extruder_in_use = e;
- //setExtruder();
- }
-}
-
-/***************************************************************************************************************************
-
-If we have a new motherboard (V 1.x, x >= 1), the extruder is entirely controlled via the RS485, and all the functions to do
-it are simple inlines in extruder.h
-
-Otherwise, we have to do the work ourselves...
-*/
-
-#if MOTHERBOARD < 2
-extruder::extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin)
-{
- motor_dir_pin = md_pin;
- motor_speed_pin = ms_pin;
- heater_pin = h_pin;
- fan_pin = f_pin;
- temp_pin = t_pin;
- valve_dir_pin = vd_pin;
- valve_en_pin = ve_pin;
- step_en_pin = se_pin;
-
- //setup our pins
- pinMode(motor_dir_pin, OUTPUT);
- pinMode(motor_speed_pin, OUTPUT);
- pinMode(heater_pin, OUTPUT);
-
- pinMode(temp_pin, INPUT);
- pinMode(valve_dir_pin, OUTPUT);
- pinMode(valve_en_pin, OUTPUT);
-
- //initialize values
- digitalWrite(motor_dir_pin, EXTRUDER_FORWARD);
-
- analogWrite(heater_pin, 0);
- analogWrite(motor_speed_pin, 0);
- digitalWrite(valve_dir_pin, false);
- digitalWrite(valve_en_pin, 0);
-
-// The step enable pin and the fan pin are the same...
-// We can have one, or the other, but not both
-
- if(step_en_pin >= 0)
- {
- pinMode(step_en_pin, OUTPUT);
- disableStep();
- } else
- {
- pinMode(fan_pin, OUTPUT);
- analogWrite(fan_pin, 0);
- }
-
- //these our the default values for the extruder.
- e_speed = 0;
- target_celsius = 0;
- max_celsius = 0;
- heater_low = 64;
- heater_high = 255;
- heater_current = 0;
- valve_open = false;
-
-//this is for doing encoder based extruder control
-// rpm = 0;
-// e_delay = 0;
-// error = 0;
-// last_extruder_error = 0;
-// error_delta = 0;
- e_direction = EXTRUDER_FORWARD;
-
- //default to cool
- set_temperature(target_celsius);
-}
-
-
-byte extruder::wait_till_hot()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() < target_celsius - HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT > oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-
-/*
-byte extruder::wait_till_cool()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() > target_celsius + HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT < oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-*/
-
-
-
-void extruder::valve_set(bool open, int dTime)
-{
- wait_for_temperature();
- valve_open = open;
- digitalWrite(valve_dir_pin, open);
- digitalWrite(valve_en_pin, 1);
- delay(dTime);
- digitalWrite(valve_en_pin, 0);
-}
-
-
-void extruder::set_temperature(int temp)
-{
- target_celsius = temp;
- max_celsius = (temp*11)/10;
-
- // If we've turned the heat off, we might as well disable the extrude stepper
- // if(target_celsius < 1)
- // disableStep();
-}
-
-/**
-* Samples the temperature and converts it to degrees celsius.
-* Returns degrees celsius.
-*/
-int extruder::get_temperature()
-{
-#ifdef USE_THERMISTOR
- int raw = sample_temperature(temp_pin);
-
- int celsius = 0;
- byte i;
-
- for (i=1; i<NUMTEMPS; i++)
- {
- if (temptable[i][0] > raw)
- {
- celsius = temptable[i-1][1] +
- (raw - temptable[i-1][0]) *
- (temptable[i][1] - temptable[i-1][1]) /
- (temptable[i][0] - temptable[i-1][0]);
-
- break;
- }
- }
-
- // Overflow: Set to last value in the table
- if (i == NUMTEMPS) celsius = temptable[i-1][1];
- // Clamp to byte
- if (celsius > 255) celsius = 255;
- else if (celsius < 0) celsius = 0;
-
- return celsius;
-#else
- return ( 5.0 * sample_temperature() * 100.0) / 1024.0;
-#endif
-}
-
-
-
-/*
-* This function gives us an averaged sample of the analog temperature pin.
-*/
-int extruder::sample_temperature()
-{
- int raw = 0;
-
- //read in a certain number of samples
- for (byte i=0; i<TEMPERATURE_SAMPLES; i++)
- raw += analogRead(temp_pin);
-
- //average the samples
- raw = raw/TEMPERATURE_SAMPLES;
-
- //send it back.
- return raw;
-}
-
-/*!
- Manages extruder functions to keep temps, speeds etc
- at the set levels. Should be called only by manage_all_extruders(),
- which should be called in all non-trivial loops.
- o If temp is too low, don't start the motor
- o Adjust the heater power to keep the temperature at the target
- */
-void extruder::manage()
-{
- //make sure we know what our temp is.
- int current_celsius = get_temperature();
- byte newheat = 0;
-
- //put the heater into high mode if we're not at our target.
- if (current_celsius < target_celsius)
- newheat = heater_high;
- //put the heater on low if we're at our target.
- else if (current_celsius < max_celsius)
- newheat = heater_low;
-
- // Only update heat if it changed
- if (heater_current != newheat) {
- heater_current = newheat;
- analogWrite(heater_pin, heater_current);
- }
-}
-
-
-#if 0
-void extruder::set_speed(float sp)
-{
- // DC motor?
- if(step_en_pin < 0)
- {
- e_speed = (byte)sp;
- if(e_speed > 0)
- wait_for_temperature();
- analogWrite(motor_speed_pin, e_speed);
- return;
- }
-
- // No - stepper
- disableTimerInterrupt();
-
- if(sp <= 1.0e-4)
- {
- disableStep();
- e_speed = 0; // Just use this as a flag
- return;
- } else
- {
- wait_for_temperature();
- enableStep();
- e_speed = 1;
- }
-
- extrude_step_count = 0;
-
- float milliseconds_per_step = 60000.0/(E_STEPS_PER_MM*sp);
- long thousand_ticks_per_step = 4*(long)(milliseconds_per_step);
- setupTimerInterrupt();
- setTimer(thousand_ticks_per_step);
- enableTimerInterrupt();
-}
-
-void extruder::interrupt()
-{
- if(!e_speed)
- return;
- extrude_step_count++;
- if(extrude_step_count > 1000)
- {
- step();
- extrude_step_count = 0;
- }
-}
-
-#endif
-#endif
-
-
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#if MOTHERBOARD > 1
-
-#include "intercom.h"
-
-intercom::intercom()
-{
- pinMode(RX_ENABLE_PIN, OUTPUT);
- pinMode(TX_ENABLE_PIN, OUTPUT);
- digitalWrite(RX_ENABLE_PIN, 0); //always listen.
- Serial1.begin(38400);
-}
-
-void intercom::sendPacket(byte address, char* string)
-{
- digitalWrite(TX_ENABLE_PIN, 1);
- Serial1.print(address, HEX);
- Serial1.print(MASTER_ADDRESS);
- Serial1.println(string);
- digitalWrite(TX_ENABLE_PIN, 0);
- getPacket(myBuffer, IC_BUFFER);
- if(myBuffer[0] != MASTER_ADDRESS[0] || myBuffer[1] != MASTER_ADDRESS[1])
- {
- // Horrible error - what to do?
- }
-}
-
-void intercom::sendPacketWithReply(byte address, char* string, char* reply)
-{
-
-}
-
-void intercom::getPacket(char* string, int len)
-{
- int i = -1;
- ok = true;
- do
- {
- while(!Serial1.available()) delay(1);
- i++;
-
- // Stop runaway buffer overflow
-
- if(i >= len)
- ok = false;
- else
- string[i] = Serial1.read();
- } while(string[i] != '\n' && ok);
- string[i] = 0;
-}
-
-#endif
-
-
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-/* bit-flags for commands and parameters */
-#define GCODE_G (1<<0)
-#define GCODE_M (1<<1)
-#define GCODE_P (1<<2)
-#define GCODE_X (1<<3)
-#define GCODE_Y (1<<4)
-#define GCODE_Z (1<<5)
-#define GCODE_I (1<<6)
-#define GCODE_J (1<<7)
-#define GCODE_K (1<<8)
-#define GCODE_F (1<<9)
-#define GCODE_S (1<<10)
-#define GCODE_Q (1<<11)
-#define GCODE_R (1<<12)
-#define GCODE_E (1<<13)
-#define GCODE_T (1<<14)
-
-
-#define PARSE_INT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_int(str, &val, &seen, flag); \
- break;
-
-#define PARSE_FLOAT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_float(str, &val, &seen, flag); \
- break;
-
-/* gcode line parse results */
-struct GcodeParser
-{
- unsigned int seen;
- int G;
- int M;
- int T;
- float P;
- float X;
- float Y;
- float Z;
- float E;
- float I;
- float J;
- float F;
- float S;
- float R;
- float Q;
-};
-
-
-//our command string
-char cmdbuffer[COMMAND_SIZE];
-char c = '?';
-byte serial_count = 0;
-boolean comment = false;
-FloatPoint fp;
-FloatPoint sp;
-
-// The following three inline functions are used for things like return to 0
-
-inline void specialMoveX(const float& x, const float& feed)
-{
- sp = where_i_am;
- sp.x = x;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveY(const float& y, const float& feed)
-{
- sp = where_i_am;
- sp.y = y;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveZ(const float& z, const float& feed)
-{
- sp = where_i_am;
- sp.z = z;
- sp.f = feed;
- qMove(sp);
-}
-
-//our feedrate variables.
-//float feedrate = SLOW_XY_FEEDRATE;
-
-/* keep track of the last G code - this is the command mode to use
- * if there is no command in the current string
- */
-int last_gcode_g = -1;
-
-boolean abs_mode = true; //0 = incremental; 1 = absolute
-
-float extruder_speed = 0;
-
-int scan_int(char *str, int *valp);
-int scan_float(char *str, float *valp);
-
-GcodeParser gc; /* string parse result */
-
-
-//init our string processing
-inline void init_process_string()
-{
- serial_count = 0;
- comment = false;
-}
-
-// Get a command and process it
-
-void get_and_do_command()
-{
- //read in characters if we got them.
- if (Serial.available())
- {
- c = Serial.read();
- if(c == '\r')
- c = '\n';
- // Throw away control chars except \n
- if(c >= ' ' || c == '\n')
- {
-
- //newlines are ends of commands.
- if (c != '\n')
- {
- // Start of comment - ignore any bytes received from now on
- if (c == ';')
- comment = true;
-
- // If we're not in comment mode, add it to our array.
- if (!comment)
- cmdbuffer[serial_count++] = c;
- }
-
- }
- }
-
- // Data runaway?
- if(serial_count >= COMMAND_SIZE)
- init_process_string();
-
- //if we've got a real command, do it
- if (serial_count && c == '\n')
- {
- // Terminate string
- cmdbuffer[serial_count] = 0;
-
- //process our command!
- process_string(cmdbuffer, serial_count);
-
- //clear command.
- init_process_string();
-
- // Say we're ready for the next one
-
- if(debugstring[0] != 0)
- {
- Serial.print("ok ");
- Serial.println(debugstring);
- debugstring[0] = 0;
- } else
- Serial.println("ok");
- }
-}
-
-
-
-int parse_string(struct GcodeParser * gc, char instruction[ ], int size)
-{
- int ind;
- int len; /* length of parameter argument */
-
- gc->seen = 0;
-
- len=0;
- /* scan the string for commands and parameters, recording the arguments for each,
- * and setting the seen flag for each that is seen
- */
- for (ind=0; ind<size; ind += (1+len))
- {
- len = 0;
- switch (instruction[ind])
- {
- PARSE_INT('G', &instruction[ind+1], len, gc->G, gc->seen, GCODE_G);
- PARSE_INT('M', &instruction[ind+1], len, gc->M, gc->seen, GCODE_M);
- PARSE_INT('T', &instruction[ind+1], len, gc->T, gc->seen, GCODE_T);
- PARSE_FLOAT('S', &instruction[ind+1], len, gc->S, gc->seen, GCODE_S);
- PARSE_FLOAT('P', &instruction[ind+1], len, gc->P, gc->seen, GCODE_P);
- PARSE_FLOAT('X', &instruction[ind+1], len, gc->X, gc->seen, GCODE_X);
- PARSE_FLOAT('Y', &instruction[ind+1], len, gc->Y, gc->seen, GCODE_Y);
- PARSE_FLOAT('Z', &instruction[ind+1], len, gc->Z, gc->seen, GCODE_Z);
- PARSE_FLOAT('I', &instruction[ind+1], len, gc->I, gc->seen, GCODE_I);
- PARSE_FLOAT('J', &instruction[ind+1], len, gc->J, gc->seen, GCODE_J);
- PARSE_FLOAT('F', &instruction[ind+1], len, gc->F, gc->seen, GCODE_F);
- PARSE_FLOAT('R', &instruction[ind+1], len, gc->R, gc->seen, GCODE_R);
- PARSE_FLOAT('Q', &instruction[ind+1], len, gc->Q, gc->seen, GCODE_Q);
- PARSE_FLOAT('E', &instruction[ind+1], len, gc->E, gc->seen, GCODE_E);
- default:
- break;
- }
- }
-}
-
-
-//Read the string and execute instructions
-void process_string(char instruction[], int size)
-{
- //the character / means delete block... used for comments and stuff.
- if (instruction[0] == '/')
- return;
-
- float fr;
-
- fp.x = 0.0;
- fp.y = 0.0;
- fp.z = 0.0;
- fp.e = 0.0;
- fp.f = 0.0;
-
- //get all our parameters!
- parse_string(&gc, instruction, size);
- /* if no command was seen, but parameters were, then use the last G code as
- * the current command
- */
- if ((!(gc.seen & (GCODE_G | GCODE_M | GCODE_T))) &&
- ((gc.seen != 0) &&
- (last_gcode_g >= 0))
- )
- {
- /* yes - so use the previous command with the new parameters */
- gc.G = last_gcode_g;
- gc.seen |= GCODE_G;
- }
- //did we get a gcode?
- if (gc.seen & GCODE_G)
- {
- last_gcode_g = gc.G; /* remember this for future instructions */
- fp = where_i_am;
- if (abs_mode)
- {
- if (gc.seen & GCODE_X)
- fp.x = gc.X;
- if (gc.seen & GCODE_Y)
- fp.y = gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z = gc.Z;
- if (gc.seen & GCODE_E)
- fp.e = gc.E;
- }
- else
- {
- if (gc.seen & GCODE_X)
- fp.x += gc.X;
- if (gc.seen & GCODE_Y)
- fp.y += gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z += gc.Z;
- if (gc.seen & GCODE_E)
- fp.e += gc.E;
- }
-
- // Get feedrate if supplied - feedrates are always absolute???
- if ( gc.seen & GCODE_F )
- fp.f = gc.F;
-
- // Process the buffered move commands first
- // If we get one, return immediately
-
- switch (gc.G)
- {
- //Rapid move
- case 0:
- fr = fp.f;
- fp.f = FAST_XY_FEEDRATE;
- qMove(fp);
- fp.f = fr;
- return;
-
- // Controlled move
- case 1:
- qMove(fp);
- return;
-
- //go home.
- case 28:
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x - 5, FAST_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 250, FAST_XY_FEEDRATE);
- where_i_am.x = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x + 1, SLOW_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 10, SLOW_XY_FEEDRATE);
- where_i_am.x = 0;
-
- specialMoveY(where_i_am.y - 5, FAST_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 250, FAST_XY_FEEDRATE);
- where_i_am.y = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveY(where_i_am.y + 1, SLOW_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 10, SLOW_XY_FEEDRATE);
- where_i_am.y = 0;
-
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z - 0.5, FAST_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 250, FAST_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z + 1, SLOW_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 2, SLOW_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_XY_FEEDRATE; // Most sensible feedrate to leave it in
-
- return;
-
-
- default:
- break;
- }
-
- // Non-buffered G commands
- // Wait till the buffer q is empty first
-
- while(!qEmpty()) delay(WAITING_DELAY);
- switch (gc.G)
- {
-
- //Dwell
- case 4:
- delay((int)(gc.P + 0.5));
- break;
-
- //Inches for Units
- case 20:
- setUnits(false);
- break;
-
- //mm for Units
- case 21:
- setUnits(true);
- break;
-
- //Absolute Positioning
- case 90:
- abs_mode = true;
- break;
-
- //Incremental Positioning
- case 91:
- abs_mode = false;
- break;
-
- //Set position as fp
- case 92:
- setPosition(fp);
- break;
-
- default:
- Serial.print("huh? G");
- Serial.println(gc.G, DEC);
- }
- }
-
-
-
-
- //find us an m code.
- if (gc.seen & GCODE_M)
- {
- // Wait till the q is empty first
- while(!qEmpty()) delay(WAITING_DELAY);
-
- switch (gc.M)
- {
- //TODO: this is a bug because search_string returns 0. gotta fix that.
- case 0:
- break;
- /*
- case 0:
- //todo: stop program
- break;
-
- case 1:
- //todo: optional stop
- break;
-
- case 2:
- //todo: program end
- break;
- */
-
-// Now, with E codes, there is no longer any idea of turning the extruder on or off.
-// (But see valve on/off below.)
-
-/*
- //turn extruder on, forward
- case 101:
- ex[extruder_in_use]->set_direction(1);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder on, reverse
- case 102:
- ex[extruder_in_use]->set_direction(0);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder off
- case 103:
- ex[extruder_in_use]->set_speed(0);
- break;
-*/
- //custom code for temperature control
- case 104:
- if (gc.seen & GCODE_S)
- {
- ex[extruder_in_use]->set_temperature((int)gc.S);
- }
- break;
-
- //custom code for temperature reading
- case 105:
- Serial.print("T:");
- Serial.println(ex[extruder_in_use]->get_temperature());
- break;
-
- //turn fan on
- case 106:
- ex[extruder_in_use]->set_cooler(255);
- break;
-
- //turn fan off
- case 107:
- ex[extruder_in_use]->set_cooler(0);
- break;
-/*
-// Extruder speed is now entirely controlled by E codes
- //set max extruder speed, 0-255 PWM
- case 108:
- if (gc.seen & GCODE_S)
- extruder_speed = gc.S;
- break;
-*/
-
-// The valve (real, or virtual...) is now the way to control any extruder (such as
-// a pressurised paste extruder) that cannot move using E codes.
-
- // Open the valve
- case 126:
- ex[extruder_in_use]->valve_set(true, (int)(gc.P + 0.5));
- break;
-
- // Close the valve
- case 127:
- ex[extruder_in_use]->valve_set(false, (int)(gc.P + 0.5));
- break;
-
-
- default:
- Serial.print("Huh? M");
- Serial.println(gc.M, DEC);
- }
-
-
-
- }
-
-// Tool (i.e. extruder) change?
-
- if (gc.seen & GCODE_T)
- {
- while(!qEmpty()) delay(WAITING_DELAY);
- new_extruder(gc.T);
- }
-}
-
-int scan_float(char *str, float *valp, unsigned int *seen, unsigned int flag)
-{
- float res;
- int len;
- char *end;
-
- res = (float)strtod(str, &end);
-
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-int scan_int(char *str, int *valp, unsigned int *seen, unsigned int flag)
-{
- int res;
- int len;
- char *end;
-
- res = (int)strtol(str, &end, 10);
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-
-
-
-int main(void)
-{
- init();
-
- setup();
-
- for (;;)
- loop();
-
- return 0;
-}
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.eep b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.eep
deleted file mode 100644
index 1996e8fd..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.eep
+++ /dev/null
@@ -1 +0,0 @@
-:00000001FF
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.elf b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.elf
deleted file mode 100755
index ee582b94..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.elf
+++ /dev/null
Binary files differ
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.hex b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.hex
deleted file mode 100644
index feef6048..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.hex
+++ /dev/null
@@ -1,1107 +0,0 @@
-:100000000C9495000C94BD000C94BD000C94BD00A4
-:100010000C94BD000C94BD000C94BD000C94BD006C
-:100020000C94BD000C94BD000C94BD000C94BD005C
-:100030000C94BD000C94660E0C94BD000C94BD0095
-:100040000C94BD000C94BD000C94491E0C94BD0092
-:100050000C94CF1D0C94BD000C94BD000C94BD00FD
-:100060000C94BD000C94BD000C94BD000C94BD001C
-:100070000C940C1E0C94BD000C94BD000021242790
-:100080002A002225282B0020232629020202020210
-:10009000020202040404040404040403030303032B
-:1000A0000303030101010101010101010204081020
-:1000B0002040800102040810204080010204081042
-:1000C000204080804020100804020100000001024E
-:1000D000000000000000000403070600000000000C
-:1000E0000000000000000000000000494E46494E9C
-:1000F0004954594E414ECDCCCC3D0AD7233C17B77D
-:10010000D13877CC2B329595E6241FB14F0A0000E9
-:1001100020410000C84200401C4620BCBE4CCA1B07
-:100120000E5AAEC59D74FE09D01C11241FBECFEF20
-:10013000D0E1DEBFCDBF11E0A0E0B1E0E0EFF4E43C
-:1001400002C005900D92AE32B107D9F716E0AEE2CB
-:10015000B1E001C01D92AB32B107E1F711E0CAE294
-:10016000D1E004C02297FE010E943022C632D1079E
-:10017000C9F70E94781A0C9476220C940000A0E033
-:10018000B0E0E5ECF0E00C944522EC015B01611578
-:10019000710519F0FB0191838083F9908F2D90E018
-:1001A0000E943422892BC9F7FDE2FF1621F4F99051
-:1001B000EE24E39405C02BE2F21609F4F990EE2444
-:1001C0008E0101501040C8016BEE70E043E050E03A
-:1001D0000E944E03892B01F58E010E5F1F4FC8014F
-:1001E0006EEE70E045E050E00E944E03892B19F45A
-:1001F0008E01095F1F4FA114B10419F0F50111839D
-:100200000083E0FC04C070E090E080E814C070E07F
-:1002100090E080E86FEF08C1C80163EF70E043E051
-:1002200050E00E944E03892B69F4A114B10429F413
-:1002300070E090E080EC6FE7F7C02296F501D18383
-:10024000C083F6CF8824992440E050E060E070E05D
-:10025000EF2DE053EA30A0F5F2E0EF2A8E2D90E08A
-:100260009C0128703070E2FE06C0232B79F50894BB
-:10027000811C911C2BC0232B19F008948108910834
-:10028000DB01CA0112E0880F991FAA1FBB1F1A9534
-:10029000D1F7480F591F6A1F7B1F440F551F661F58
-:1002A000771F4E0F511D611D711D483929E95207F5
-:1002B00029E9620729E1720748F084E0E82A06C0CC
-:1002C000EE3F31F4E3FC39C098E0E92AF990C0CF61
-:1002D000E53311F0E53189F529912D3219F4E0E18A
-:1002E000EE2A05C02B3219F081E090E003C029917D
-:1002F00082E090E0E22FE053EA3018F0C81BD90BFF
-:100300001CC020E030E0FCE020383F075CF4C9016D
-:10031000880F991F880F991F280F391F220F331F2D
-:100320002E0F311DE991E053EA3068F3E4FE03C07B
-:10033000309521953F4F820E931ECE2CDD24E1FE99
-:1003400007C0A114B10421F02197F501D183C08326
-:10035000CB01BA010E942D207B018C01F3E0CF225A
-:10036000DD2423E0C216D10421F417FB109517F900
-:10037000109557016801C801B70120E030E040E066
-:1003800050E00E94901F882309F44AC097FE0DC0D8
-:100390002AE0E22E21E0F22E909481949108939429
-:1003A000C0E2D0E000E010E012C092E2E92E91E05D
-:1003B000F92EF6CFF7012591359145915491C6015B
-:1003C000B5010E94E3205B016C018C1A9D0A8C161A
-:1003D0009D0684F7D595C7950F5F1F4F0630110511
-:1003E00029F08CEF9FEFE80EF91EF1CFC501D60181
-:1003F0007C018D018C2D880F8D2D881F8F3F51F032
-:10040000C601B50120E030E040E050E00E94901FBE
-:10041000882331F482E290E090932A0680932906A3
-:100420007E2D9F2D802F612F272F392F482F562F5C
-:10043000B901CA01CDB7DEB7ECE00C9461222F926E
-:100440003F925F926F927F928F929F92AF92BF92F4
-:10045000CF92DF92EF92FF920F931F93CF93DF9390
-:100460008C011B01EA016115710519F0FB019183F3
-:100470008083209749F0CE010297839728F020E0EF
-:1004800030E040E050E0F6C0F801A1908F018A2DE5
-:1004900090E00E943422892BB9F7FDE2AF1631F4C7
-:1004A000F801A1908F015524539407C0FBE2AF16C9
-:1004B00019F4F801A1908F015524209719F0C0314B
-:1004C000D105C1F4F0E3AF1679F4F80180818837E3
-:1004D00011F0883549F4F801A1800E5F1F4FF2E05A
-:1004E0005F2AC0E1D0E006C0209721F480E3A8167F
-:1004F000E9F427C0C830D10531F1C930D10524F461
-:10050000C230D10531F50CC0CA30D10589F0C031F7
-:10051000D105F9F4C12CD12CE12CB8E0FB2E28C078
-:10052000C12CD12CE12CA0E4FA2E22C0CAE0D0E0EC
-:10053000FCECCF2EFCECDF2EFCECEF2EFCE0FF2ED3
-:1005400017C0C8E0D0E0C12CD12CE12CE0E1FE2E98
-:100550000FC09E01442737FD4095542F60E070E0A6
-:1005600080E090E80E94DD21C901DA016C017D0183
-:1005700020E030E040E050E060E03E01882477FC7D
-:100580008094982C70EDB72EBA0CE9E0EB1570F45E
-:100590008A2D81548A3118F499ECB92E06C08A2D1F
-:1005A00081568A3150F589EAB82EBA0C8B2D90E02D
-:1005B0008C179D0714F56F3FE1F0C216D306E406D1
-:1005C000F506B0F0CA01B901A40193010E94AB2164
-:1005D0009B01AC012B0D311D411D511D2130F0E05F
-:1005E0003F07F0E04F07F0E85F0710F461E001C05B
-:1005F0006FEFF801A1908F01C5CF2114310481F074
-:10060000662331F001501040F1011183008308C0CE
-:1006100051FE1AC002501040F1011183008314C032
-:1006200067FF12C050FC05C02FEF3FEF4FEF5FE7B1
-:1006300004C020E030E040E050E882E290E0909397
-:100640002A068093290616C050FE08C05095409592
-:10065000309521953F4F4F4F5F4F0CC057FF0AC059
-:1006600082E290E090932A06809329062FEF3FEFD5
-:100670004FEF5FE7B901CA01DF91CF911F910F9151
-:10068000FF90EF90DF90CF90BF90AF909F908F90B2
-:100690007F906F905F903F902F900895FB01DC0159
-:1006A0004150504088F08D9181341CF08B350CF412
-:1006B000805E659161341CF06B350CF4605E861BC6
-:1006C000611171F3990B0895881BFCCF2F923F9213
-:1006D0004F925F926F927F928F929F92AF92BF9252
-:1006E000CF92DF92EF92FF920F931F93DF93CF93FE
-:1006F000CDB7DEB72E970FB6F894DEBF0FBECDBFD5
-:100700009E878D878B017A01FB01608171818281D7
-:100710009381FA0120813181428153810E94E3203B
-:100720005B016C01F8016481758186819781F70115
-:1007300024813581468157810E94E3203B014C0191
-:10074000F8016085718582859385F7012085318563
-:10075000428553850E94E3201B012C01F80164852A
-:10076000758586859785F701248535854685578566
-:100770000E94E32069877A878B879C87F8016089CC
-:10078000718982899389F701208931894289538946
-:100790000E94E3207B018C01C601B50120E030E01E
-:1007A00040E050E00E94DF2088234CF0C601B501F4
-:1007B00020E030E040E05FE30E942C1F08C0C6014B
-:1007C000B50120E030E040E05FE30E942B1F0E9473
-:1007D000FC1F69837A838B839C83C401B30120E06F
-:1007E00030E040E050E00E94DF2088234CF0C4015C
-:1007F000B30120E030E040E05FE30E942C1F08C01E
-:10080000C401B30120E030E040E05FE30E942B1F11
-:100810000E94FC1F6D837E838F839887C201B10184
-:1008200020E030E040E050E00E94DF2088234CF0E0
-:10083000C201B10120E030E040E05FE30E942C1FE4
-:1008400008C0C201B10120E030E040E05FE30E9457
-:100850002B1F0E94FC1F3B014C0169857A858B850B
-:100860009C8520E030E040E050E00E94DF208823BB
-:100870005CF069857A858B859C8520E030E040E0DE
-:100880005FE30E942C1F0AC069857A858B859C8551
-:1008900020E030E040E05FE30E942B1F0E94FC1F3D
-:1008A0005B016C01C801B70120E030E040E050E09E
-:1008B0000E94DF2088234CF0C801B70120E030E01F
-:1008C00040E05FE30E942C1F08C0C801B70120E090
-:1008D00030E040E05FE30E942B1F0E94FC1F298153
-:1008E0003A814B815C81ED85FE8520833183428393
-:1008F00053832D813E814F815885248335834683E0
-:1009000057836086718682869386A486B586C6865E
-:10091000D786608B718B828B938BCF012E960FB60F
-:10092000F894DEBF0FBECDBFCF91DF911F910F9125
-:10093000FF90EF90DF90CF90BF90AF909F908F90FF
-:100940007F906F905F904F903F902F9008958823F5
-:1009500021F4809181008B7F29C0813049F48091FE
-:1009600081008B7F8093810080918100826015C01F
-:10097000823049F4809181008B7F80938100809147
-:100980008100826018C0833071F48091810084609E
-:1009900080938100809181008D7F80938100809180
-:1009A00081008E7F0DC080918100846080938100E2
-:1009B000809181008D7F8093810080918100816092
-:1009C000809381000895E0E8F0E01082A1E8B0E0B3
-:1009D0001C921092820010926F008C918F7E8C93EB
-:1009E0008C9188608C9380818D7F808380818E7FC5
-:1009F000808380818F77808380818F7B80838081DB
-:100A00008F7D808380818F7E808384E00E94A70415
-:100A10008FEF9FEF90938900809388000895FC01E9
-:100A200080819181A281B381803020E0920721E012
-:100A3000A20720E0B2070CF441C0893F2FEF9207D4
-:100A400027E0A20720E0B20744F453E0B595A7954C
-:100A5000979587955A95D1F731C0813C2FEF920732
-:100A60002FE3A20720E0B20744F446E0B595A7952E
-:100A7000979587954A95D1F721C081302FEF92073E
-:100A80002FEFA20720E0B2073CF4892F9A2FAB2F5B
-:100A9000BB27A7FDBA9512C081302CEF92072FEF2C
-:100AA000A20723E0B2071CF0EFEFFFEF08C02AE037
-:100AB000B595A795979587952A95D1F7FC01CF0114
-:100AC0000895EF92FF920F931F93DF93CF9300D07F
-:100AD00000D0CDB7DEB77B018C0164E0EE0CFF1CCB
-:100AE000001F111F6A95D1F7E982FA820B831C83DC
-:100AF000CE0101960E940F05909389008093880093
-:100B000080E0E81680E0F80681E0080780E018073A
-:100B100014F480E022C089EFE8168FEFF80687E032
-:100B2000080780E0180714F481E017C081ECE8168C
-:100B30008FEFF8068FE3080780E0180714F482E0CF
-:100B40000CC081E0E8168FEFF8068FEF080780E011
-:100B5000180714F084E001C083E00E94A7040F90FE
-:100B60000F900F900F90CF91DF911F910F91FF90F9
-:100B7000EF900895AF92BF92CF92DF92EF92FF92E3
-:100B80000F931F93DF93CF9300D0CDB7DEB78C01C7
-:100B90007B016A015901BE016F5F7F4F4AE050E05F
-:100BA0000E941F0229813A81201B310B1216130665
-:100BB0005CF4F70171836083F601808191818A2959
-:100BC0009B299183808303C0F70111821082C901A0
-:100BD0000F900F90CF91DF911F910F91FF90EF90A9
-:100BE000DF90CF90BF90AF900895AF92BF92CF9219
-:100BF000DF92EF92FF920F931F93DF93CF9300D07A
-:100C0000CDB7DEB78C017B016A015901BE016F5F70
-:100C10007F4F0E94BF0029813A81201B310B1216A1
-:100C200013066CF4F7016083718382839383F6016A
-:100C3000808191818A299B299183808309C080E0EA
-:100C400090E0A0E0B0E0F70180839183A283B383BA
-:100C5000C9010F900F90CF91DF911F910F91FF90DD
-:100C6000EF90DF90CF90BF90AF9008952F923F927A
-:100C70004F925F926F927F928F929F92AF92BF92AC
-:100C8000CF92DF92EF92FF920F931F93DF93CF9358
-:100C9000CDB7DEB764970FB6F894DEBF0FBECDBFF9
-:100CA0008B015A8B498BFC01119211926F017C01CF
-:100CB0009C012C5F3F4F388B2F87FC013696FE87B7
-:100CC000ED879C01285D3F4F3C872B87FC013896C0
-:100CD000FA87E9879C01245F3F4F38872F83FC0107
-:100CE0007096FE83ED839C012C5E3F4F3C832B83EB
-:100CF000FC017C96FA83E983B0E22B2E312C280E7E
-:100D0000391EA4E24A2E512C480E591EFCE26F2EC9
-:100D1000712C680E791EE0E38E2E912C880E991EA0
-:100D200078E1A72EB12CA80EB91E1C8A1B8ACCC054
-:100D3000EB89FC89E00FF11FE081E03509F456C032
-:100D4000E13594F4E73441F1E83434F4E53409F45E
-:100D5000A9C0E634F1F48BC0EA3409F47EC0ED3466
-:100D600029F1E934B1F46EC0E43569F1E5354CF4AC
-:100D7000E23509F486C08B899C890196E33574F5C8
-:100D80008AC0E93509F448C0EA3509F450C0E835AD
-:100D9000C1F180E090E091C08B899C890196800F21
-:100DA000911FB601A70121E030E00AC08B899C8920
-:100DB0000196800F911F6F857889A70122E030E0AE
-:100DC0000E94BA057AC08B899C890196800F911F79
-:100DD0006D857E85A70120E030E4F2CF800F911F62
-:100DE0006B857C85A70120E034E065C08B899C89F8
-:100DF0000196800F911F69857A85A70124E030E074
-:100E00005AC08B899C890196800F911F6F817885CC
-:100E1000A70128E030E04FC08B899C890196800FA4
-:100E2000911F6D817E81A70120E130E044C08B8954
-:100E30009C890196800F911F6B817C81A70120E224
-:100E400030E039C08B899C890196800F911F6981A0
-:100E50007A81A70120E430E02EC08B899C8901961D
-:100E6000800F911FB101A70120E830E024C08B89D9
-:100E70009C890196800F911FB201A70120E032E00A
-:100E80001AC08B899C890196800F911FB301A7011D
-:100E900020E030E110C0800F911FB401A70120E0D5
-:100EA00038E009C08B899C890196800F911FB5019C
-:100EB000A70120E030E20E94F50501962B893C89CC
-:100EC000280F391F3C8B2B8B8B899C89E989FA89E8
-:100ED0008E179F070CF42CCF64960FB6F894DEBFE4
-:100EE0000FBECDBFCF91DF911F910F91FF90EF907B
-:100EF000DF90CF90BF90AF909F908F907F906F903A
-:100F00005F904F903F902F900895EF92FF920F9334
-:100F10001F937C01FC0185890E94A81B8C01F701AD
-:100F200085890E94A81B080F191FF70185890E9457
-:100F3000A81B800F911F63E070E00E94CA21CB01C3
-:100F40001F910F91FF90EF9008950E948507AA27A7
-:100F500097FDA095BA2FBC01CD010E942F2020E063
-:100F600030E040EA50E40E94E32020E030E048EC2A
-:100F700052E40E94E32020E030E040E85AE30E947F
-:100F8000E3200E94FC1FCB010895CF93DF93EC0177
-:100F90000E94A5079C0189819A812817390714F4BA
-:100FA0006E8108C08B819C812817390714F060E09E
-:100FB00001C06D818F81861729F06F838B8970E066
-:100FC0000E94C61BDF91CF91089580912E019091D0
-:100FD0002F010E94C50780913001909131010E943C
-:100FE000C5070895CF93DF93EC011A860E94A507E9
-:100FF0009C878B871DC00E94E5078A858F5F8A8743
-:10100000853180F0CE010E94A5079C019E878D87C7
-:101010008B859C858217930714F081E014C03C8770
-:101020002B871A8668EE73E080E090E00E94B91E7C
-:10103000CE010E94A50729813A81255030408217B0
-:101040009307CCF280E0DF91CF910895FF920F9348
-:101050001F93CF93DF93EC01F62E8A010E94F207D3
-:10106000882371F08FE095E060E071E00E947F1AC4
-:10107000CE010E94A507BC018FE095E00E94781B7D
-:10108000F88A8E896F2D0E94391C8F8961E00E9439
-:10109000391CB801882777FD8095982F0E94B91ECA
-:1010A0008F8960E00E94391CDF91CF911F910F91D1
-:1010B000FF90089583E161E00E94391C83E161E0C3
-:1010C0000E94391C0895CF93DF93EC01C859DF4F7C
-:1010D00088819981AA81BB81C856D0400097A1051B
-:1010E000B10521F083E160E00E94391CC459DF4F53
-:1010F00088819981AA81BB81CC56D0400097A105F7
-:10110000B10521F083E160E00E94391CC059DF4F36
-:1011100088819981AA81BB81C057D0400097A105E1
-:10112000B10521F08FE160E00E94391CCC58DF4FFF
-:1011300088819981AA81BB810097A105B10571F0D1
-:10114000E0913201F0E0EE0FFF1FE25DFE4F0190F3
-:10115000F081E02D808D60E00E94391CDF91CF91FD
-:101160000895CF93DF93EC01CA55DF4F8881C65AAB
-:10117000D0408823E1F5CC56DF4F82E169910E948F
-:10118000391C86E169910E94391C8EE169910E94A7
-:10119000391C8881C759D040E0913201882369F019
-:1011A000F0E0EE0FFF1FE25DFE4F0190F081E02DB9
-:1011B00081E08787818961E00BC0F0E0EE0FFF1FBF
-:1011C000E25DFE4F0190F081E02D1786818960E09D
-:1011D0000E94391CCE010E94630868EE73E080E033
-:1011E00090E00E946105C855DF4F81E08883DF9160
-:1011F000CF9108956F927F928F92AF92CF92EF929C
-:101200000F93DF93CF93CDB7DEB73C01862FF30169
-:10121000618B428B238B048BE58AC68AA78A808EDA
-:1012200061E00E941B1CF301828961E00E941B1C8B
-:10123000F301838961E00E941B1CF301858960E052
-:101240000E941B1CF301868961E00E941B1CF301B4
-:10125000878961E00E941B1CF301818961E00E9483
-:10126000391CF301838960E070E00E94C61BF30122
-:10127000828960E070E00E94C61BF301868960E00D
-:101280000E94391CF301878960E00E94391CF30138
-:10129000808D61E00E941B1CF301808D61E00E9443
-:1012A000391CF301108280E485838FEF86831782D7
-:1012B000108A81E087871282118280E090E06AE0E4
-:1012C00070E00E94CA2174836383CF91DF910F91F4
-:1012D000EF90CF90AF908F907F906F9008950F9385
-:1012E0001F938C0108551F4FF801108202501040C7
-:1012F000F80110820251104081E0F801819381933E
-:10130000819381938F0180830859104080E090E0A1
-:10131000A0E0B0E0F801848B958BA68BB78B808F13
-:10132000918FA28FB38F848F958FA68FB78F80A355
-:1013300091A3A2A3B3A380E090E0AAE7B4E484A3BE
-:1013400095A3A6A3B7A38FE061E00E941B1C82E1D6
-:1013500061E00E941B1C87E161E00E941B1C86E18A
-:1013600061E00E941B1C8DE161E00E941B1C8EE16C
-:1013700061E00E941B1C83E161E00E941B1C83E171
-:1013800061E00E941B1C8FE161E00E941B1CC801F0
-:101390000E945A0884E160E00E941B1C89E160E021
-:1013A0000E941B1C82E060E00E941B1C8BE49AEEF2
-:1013B000AFEFB0E4F80180839183A283B383848389
-:1013C0009583A683B78380E090E0A0EAB3E48087AA
-:1013D0009187A287B3878AE69CEBA4E3BFE3848767
-:1013E0009587A687B78780E090E0A0E8BFE3808B71
-:1013F000918BA28BB38B1F910F9108958F92AF9217
-:10140000CF92EF920F9389E392E060E044E028E10D
-:1014100007E0B3E0EB2EA6E0CA2EF5E0AF2E88245D
-:101420008A940E94FA0882E592E060E14CE02EE0A6
-:1014300003E0E4E0EE2E71E1C72E5DE0A52E33E07F
-:10144000832E0E94FA088BE692E00E946F0984E1E5
-:1014500093E00E946F098DEB93E00E946F0986E68E
-:1014600094E00E946F090F91EF90CF90AF908F9012
-:101470000895AF92BF92CF92DF92EF92FF920F93B7
-:101480001F93B62EC016D106E206F30611F480E0D3
-:101490000DC0862F0E94841C892B39F08B2D0E9451
-:1014A000841C892B11F08A2DA11081E01F910F91CE
-:1014B000FF90EF90DF90CF90BF90AF9008952F9264
-:1014C0003F924F925F926F927F928F929F92AF92D4
-:1014D000BF92CF92DF92EF92FF920F931F93DF9311
-:1014E000CF93CDB7DEB7AC970FB6F894DEBF0FBE83
-:1014F000CDBF88559F4FDC018C91A85AB040BCA746
-:10150000ABA7882309F4C9C3A05CBF4FBA83A983E2
-:10151000EBA5FCA5EC5AFF4FFC83EB832BA53CA568
-:101520002C563F4F3E832D838BA59CA587569F4FFE
-:1015300098878F83ABA5BCA5AC5BBF4FBA87A98743
-:10154000EBA5FCA5E85AFF4FFC87EB872BA53CA534
-:101550002B563F4F3E872D878BA59CA586569F4FC8
-:10156000988B8F87ABA5BCA5A85BBF4FBA8BA98B07
-:10157000EBA5FCA5E45AFF4FFC8BEB8B2BA53CA500
-:101580002A563F4F3E8B2D8B8BA59CA585569F4F92
-:10159000988F8F8B3CE4232E312CABA5BCA52A0E53
-:1015A0003B1EA05ABF4FBA8FA98FEBA5FCA5E956E9
-:1015B000FF4FFC8FEB8F2BA53CA524563F4F3E8F52
-:1015C0002D8F20E5622E712C8BA59CA5680E791EAF
-:1015D0008C599F4F98A38F8FABA5BCA5A856BF4F22
-:1015E000BAA3A9A3EBA5FCA5E356FF4FFCA3EBA36D
-:1015F00097EA892E912C2BA53CA5820E931E285983
-:101600003F4F3EA32DA38BA59CA584589F4F98A721
-:101610008FA38EE9482E512CABA5BCA54A0E5B1EAC
-:10162000A459BF4FBAA7A9A7E981FA810081118106
-:1016300022813381AB81BC81CD90DD90ED90FC9017
-:101640008BA59CA564E145E1ED81FE81A0800E940F
-:10165000390AAF81B8858C93E985FA8500811181BB
-:1016600022813381AB85BC85CD90DD90ED90FC90DF
-:101670008BA59CA569E14AE1ED85FE85A0800E94CD
-:10168000390AAF85B8898C93E989FA89008111817B
-:1016900022813381AB89BC89CD90DD90ED90FC90A7
-:1016A0008BA59CA562E041E0ED89FE89A0800E94A7
-:1016B000390AAF89B88D8C93F101008111812281A3
-:1016C0003381A98DBA8DCD90DD90ED90FC908BA5E6
-:1016D0009CA56FEF4FEFEB8DFC8DA0800E94390A27
-:1016E000AD8DBE8D8C93F3010081118122813381F8
-:1016F000AF8DB8A1CD90DD90ED90FC908BA59CA511
-:101700006FEF4FEFE9A1FAA1A0800E94390AABA1C7
-:10171000BCA18C93F4011082AF81B8858C91882391
-:1017200009F45CC0EDA1FEA180819181A281B38109
-:10173000EFA1F8A52081318142815381820F931F4F
-:10174000A41FB51F80839183A283B3831816190643
-:101750001A061B060CF042C08FE061E00E94391CA3
-:1017600085E090E00E94E31E8FE060E00E94391C5B
-:1017700081E0D4018C93EFA1F8A580819181A281B1
-:10178000B381F2012081318142815381821B930B0D
-:10179000A40BB50BEFA1F8A580839183A283B3833B
-:1017A000AD81BE818C91E981FA81208131814281B4
-:1017B0005381882349F02F5F3F4F4F4F5F4F208366
-:1017C0003183428353830AC021503040404050400F
-:1017D000E981FA812083318342835383AF85B889BD
-:1017E0008C91882309F45AC00BA51CA500581F4FE3
-:1017F000E9A5FAA580819181A281B381F8012081B8
-:10180000318142815381820F931FA41FB51F8083B2
-:101810009183A283B383181619061A061B06F4F5E2
-:1018200087E161E00E94391C85E090E00E94E31EA0
-:1018300087E160E00E94391C81E0D4018C93F801BB
-:1018400080819181A281B381F20120813181428125
-:101850005381821B930BA40BB50BF80180839183FA
-:10186000A283B383AD85BE858C91882351F0E98531
-:10187000FA8580819181A281B3810196A11DB11D5C
-:1018800009C0E985FA8580819181A281B3810197A0
-:10189000A109B10980839183A283B383AF89B88DF5
-:1018A0008C91882309F460C00BA51CA50C571F4F11
-:1018B000EBA5FCA5E059FF4F80819181A281B38106
-:1018C000E057F040FCA7EBA7F8012081318142816D
-:1018D0005381820F931FA41FB51F80839183A2831E
-:1018E000B383181619061A061B06F4F58DE161E09C
-:1018F0000E94391C85E090E00E94E31E8DE160E0CB
-:101900000E94391C81E0D4018C93F801808191817F
-:10191000A281B381F2012081318142815381821BF6
-:10192000930BA40BB50BF80180839183A283B3833F
-:10193000AD89BE898C91882351F0E989FA8980812B
-:101940009181A281B3810196A11DB11D09C0E989D0
-:10195000FA8980819181A281B3810197A109B1099E
-:1019600080839183A283B383AD8DBE8D8C918823B8
-:1019700009F46CC008E8E02EF12CEBA5FCA5EE0EF6
-:10198000FF1EEC58FF4F80819181A281B381E45703
-:10199000F040FCA7EBA7F701208131814281538100
-:1019A000820F931FA41FB51F80839183A283B383EB
-:1019B000181619061A061B060CF048C0E0913201F1
-:1019C000F0E0EE0FFF1FE25DFE4F00811181D801B4
-:1019D00052968C9161E00E94391C85E090E00E9453
-:1019E000E31EF801828960E00E94391C81E0D40185
-:1019F0008C93F70180819181A281B381F2012081D2
-:101A0000318142815381821B930BA40BB50BF701EB
-:101A100080839183A283B383AB8DBC8D8C9188230B
-:101A200049F0F10180819181A281B3810196A11DCC
-:101A3000B11D08C0F10180819181A281B38101971C
-:101A4000A109B10980839183A283B383ABA1BCA117
-:101A50008C91882309F44AC0EBA5FCA5E457FF4FFD
-:101A60002BA53CA528583F4FD9012D913D914D9173
-:101A70005C911397A857B040BCA7ABA78081918118
-:101A8000A281B381280F391F4A1F5B1F2083318336
-:101A90004283538312161306140615063CF5D20131
-:101AA000ED90FD900D911C912E193F09400B510BAB
-:101AB0002083318342835383E9A1FAA18081882363
-:101AC00049F0F30180819181A281B3810196A11D2A
-:101AD000B11D08C0F30180819181A281B38101977A
-:101AE000A109B10980839183A283B383D4018C912E
-:101AF000882309F487C0EBA5FCA5E057FF4F2081A0
-:101B0000318142815381E059F040FCA7EBA78F015E
-:101B10000E551F4F22303105410551056CF0D301A0
-:101B20006D917D918D919C910E94AB21F801608314
-:101B30007183828393830AC0F30180819181A281A2
-:101B4000B381F80180839183A283B383ABA5BCA545
-:101B5000DC966D917D918D919C91DF9720EC31EE1B
-:101B600044E65CE40E94E3205B016C01EBA5FCA56C
-:101B7000EE55FF4F60817181828193810E942F20F9
-:101B80007B018C01F20160817181828193810E94CD
-:101B90002F209B01AC01C801B7010E94E3209B01EB
-:101BA000AC01C601B5010E94941F7B018C0120E0AD
-:101BB00030E040E050E00E94DF2088234CF0C80174
-:101BC000B70120E030E040E05FE30E942C1F08C036
-:101BD000C801B70120E030E040E05FE30E942B1F26
-:101BE0000E94FC1F2BA53CA52E553F4FD9016D939C
-:101BF0007D938D939C931397A25AB040BCA7ABA73B
-:101C00000E946105F4018081882331F4ABA1BCA15D
-:101C10008C91882309F008CDEBA5FCA5E756FF4F72
-:101C20008081E959F040FCA7EBA7882319F5E65617
-:101C3000FF4F8081EA59F040FCA7EBA78823D1F43D
-:101C4000E556FF4F8081EB59F040FCA7EBA78823B6
-:101C500089F4E456FF4F8081EC59F040FCA7EBA7D4
-:101C6000882341F4E356FF4F8081ED59F040FCA7F3
-:101C7000EBA701C081E0EBA5FCA5E855FF4F8083F1
-:101C80008081882351F48BA59CA50E945A0868EE98
-:101C900073E080E090E00E946105AC960FB6F89486
-:101CA000DEBF0FBECDBFCF91DF911F910F91FF908F
-:101CB000EF90DF90CF90BF90AF909F908F907F90EC
-:101CC0006F905F904F903F902F9008951F920F92CA
-:101CD0000FB60F9211241F932F933F934F935F934F
-:101CE0006F937F938F939F93AF93BF93EF93FF93E4
-:101CF00080916F008D7F80936F00E0914601F0E04E
-:101D0000EE0FFF1FE35CFE4F0190F081E02DE855E0
-:101D1000FF4F8081882361F0E0914601F0E0EE0FF3
-:101D2000FF1FE35CFE4F808191810E945F0A22C009
-:101D30009091460180914501981799F5E0914601EF
-:101D4000F0E0EE0FFF1FE35CFE4F0190F081E02D0D
-:101D5000E855FF4F8081882321F50CC0E12FF0E08A
-:101D6000EE0FFF1FE35CFE4F808191810E94B1085E
-:101D70001093460180916F00826080936F00FF9105
-:101D8000EF91BF91AF919F918F917F916F915F91F3
-:101D90004F913F912F911F910F900FBE0F901F9069
-:101DA0001895109146011F5F1430C0F210E0D6CF95
-:101DB0002F923F924F925F926F927F928F929F925B
-:101DC000AF92BF92CF92DF92EF92FF920F931F9349
-:101DD000DF93CF93CDB7DEB7A4970FB6F894DEBFED
-:101DE0000FBECDBF2C01362E272EDC015496862F38
-:101DF000972F9C01F90184E101900D928150E1F748
-:101E000026EA30E0420E531ED2011C92EAE5FFEFB3
-:101E10004E0E5F1ED20154966D917D918D919C91D5
-:101E200057972091470130914801409149015091C5
-:101E30004A010E942B1F5B016C01E894D7F8F20164
-:101E4000608D718D828D938D20914B0130914C016D
-:101E500040914D0150914E010E942B1F7B018C013E
-:101E6000E89417F9D2015C966D917D918D919C91CA
-:101E70005F9720914F013091500140915101509155
-:101E800052010E942B1F3B014C01E89497F8F2018C
-:101E900060A171A182A193A12091530130915401BD
-:101EA00040915501509156010E942B1F9B01AC019E
-:101EB0005F772D8F3E8F4F8F58A3D20194966D91EF
-:101EC0007D918D919C919797209157013091580168
-:101ED0004091590150915A010E942B1F9B01AC0166
-:101EE0005F7729A33AA34BA35CA3D201D8962D9385
-:101EF0003D934D935C93DB972D8D3E8D4F8D58A177
-:101F0000D4962D933D934D935C93D797F20160AA9D
-:101F100071AA82AA93AA9C96ED92FD920D931C93AE
-:101F20009F97A0A6B1A6C2A6D3A6C601B501A601D9
-:101F300095010E94E3205B016C01C801B701A80173
-:101F400097010E94E3209B01AC01C601B5010E94EC
-:101F50002C1F7B018C01C401B301A40193010E94D9
-:101F6000E3209B01AC01C801B7010E942C1FD201E4
-:101F7000DC966D937D938D939C93DF9720E030E00A
-:101F800040E050E00E94901F18166CF06D8D7E8D21
-:101F90008F8D98A19B01AC010E94E320F20164AFF8
-:101FA00075AF86AF97AFD201DC966D917D918D9123
-:101FB0009C91DF9720E030E040E050E00E94901FCD
-:101FC00018166CF069A17AA18BA19CA19B01AC01B0
-:101FD0000E94E320F20164AF75AF86AF97AFD201E4
-:101FE000DC966D917D918D919C91DF970E944A21A5
-:101FF000F20164AF75AF86AF97AFF42CE52C8E017C
-:102000000F5F1F4FC801B20147E451E00E94660311
-:10201000D201A05CBF4FF80184E101900D92815084
-:10202000E1F724E130E0420E531E8E010F5F1F4F97
-:10203000C8016F2D7E2DA2010E94660340E450E08E
-:10204000440E551ED2016CEA7FEF460E571EF80172
-:1020500084E101900D928150E1F7F201EC5AFF4FBB
-:1020600080E490E0480E591E2081318142815381E5
-:102070002D8B3E8B4F8B588FF20180819181A281F5
-:10208000B38120EC3FEF420E531E4D895E896F896C
-:10209000788D481B590B6A0B7B0B4D8B5E8B6F8BBE
-:1020A000788F77FF0BC070956095509541955F4F85
-:1020B0006F4F7F4F4D8B5E8B6F8B788FF201E85A9D
-:1020C000FF4F64E470E0460E571EE080F18002810D
-:1020D0001381F20180819181A281B3812CEB3FEFCA
-:1020E000420E531EE81AF90A0A0B1B0B17FF08C011
-:1020F00010950095F094E094E11CF11C011D111D58
-:10210000F201E45AFF4F48E450E0440E551E60814E
-:10211000718182819381698F7A8F8B8F9C8FF2017D
-:1021200080819181A281B38128EB3FEF420E531E43
-:10213000498D5A8D6B8D7C8D481B590B6A0B7B0B1F
-:10214000498F5A8F6B8F7C8F77FF0BC0709560958E
-:10215000509541955F4F6F4F7F4F498F5A8F6B8FCF
-:102160007C8FF201E05AFF4F6CE470E0460E571E80
-:1021700080809180A280B380F20180819181A281D0
-:10218000B38124EB3FEF420E531E881A990AAA0A24
-:10219000BB0AB7FE08C0B094A09490948094811CB0
-:1021A000911CA11CB11CB20168587F4FF201EC597F
-:1021B000FF4F40E550E0440E551E208131814281A1
-:1021C0005381F20180819181A281B381E0EBFFEF25
-:1021D0004E0E5F1E281B390B4A0B5B0B57FF07C0C7
-:1021E00050954095309521953F4F4F4F5F4FDB0104
-:1021F0002D933D934D935C931397E4E7F0E04E0EDF
-:102200005F1ED2018D929D92AD92BC921397298D43
-:102210003A8D4B8D5C8D5E934E933E932E93FD01D4
-:1022200012930293F292E292DF012D893E894F8947
-:10223000588D5E934E933E932E932D01E8E9FFEF68
-:102240004E0E5F1EDA01C9012E153F0540075107EA
-:1022500014F4D801C7012EE930E0420E531EF201FA
-:1022600080839183A283B38322ED3FEF420E531EFE
-:10227000F201208131814281538160E97FEF460E76
-:10228000571E82179307A407B50714F4DA01C90192
-:10229000EEE9F0E04E0E5F1EF20180839183A2838F
-:1022A000B38326ED3FEF420E531EF20120813181B0
-:1022B000428153816CE87FEF460E571E82179307C9
-:1022C000A407B50714F4DA01C901EEE9F0E04E0EF7
-:1022D0005F1EF20180839183A283B38322E63FEFE6
-:1022E000420E531E0097A105B10599F446EA50E04D
-:1022F000440E551E81E0D2018C93A7E4B1E0232D5A
-:10230000322DC901FC0184E101900D928150E1F769
-:10231000A1C1B20168587F4FF201EC59FF4F20E58F
-:1023200030E0420E531E2081318142815381F201FF
-:1023300080819181A281B381E0EBFFEF4E0E5F1EA1
-:10234000281B390B4A0B5B0B57FF07C05095409574
-:10235000309521953F4F4F4F5F4FDB012D933D93BC
-:102360004D935C931397F0E9CF2ED12CC40CD51C60
-:1023700021E030E040E050E0D6012D933D934D93B5
-:102380005C931397E8E7F0E04E0E5F1ED201ED90EC
-:10239000FD900D911C91E8E8FFEF4E0E5F1EEEE9F7
-:1023A0006E2E712C640C751CD3012D913D914D91B5
-:1023B0005C912E153F05400751070CF075C0C80110
-:1023C000B7010E94FF2149015A01F60180829182E2
-:1023D000A282B382F3E08F169104A104B1040CF43D
-:1023E00054C082010C591F4FD8016D917D918D9180
-:1023F0009C91A50194010E94FF2169017A01F801D5
-:1024000020833183428353838201005B1F4FD801B5
-:102410006D917D918D919C91A50194010E94FF2168
-:10242000C901DA01F80180839183A283B383A701F4
-:102430009601281B390B4A0B5B0B57FF07C05095C1
-:102440004095309521953F4F4F4F5F4F68E770E0C3
-:10245000460E571ED2012D933D934D935C931397D7
-:10246000E8E8FFEF4E0E5F1EF201E256FF4F80815B
-:102470009181A281B38182179307A407B507A4F4C1
-:1024800020833183428353830FC021E030E040E05A
-:1024900050E0D6012D933D934D935C931397F30138
-:1024A000E082F1820283138382010C561F4FFF24C6
-:1024B000D20154966D917D918D919C915797209169
-:1024C0004701309148014091490150914A010E94D1
-:1024D000DF20882314F0FF24F394F801F0828201B6
-:1024E0000B561F4FFF24D20158966D917D918D910F
-:1024F0009C915B9720914B0130914C0140914D0193
-:1025000050914E010E94DF20882314F0FF24F394A1
-:10251000F801F08282010A561F4FFF24D2015C9617
-:102520006D917D918D919C915F9720914F0130919C
-:10253000500140915101509152010E94DF208823A7
-:1025400014F0FF24F394F801F082820109561F4F22
-:10255000FF24D20190966D917D918D919C919397DE
-:1025600020915301309154014091550150915601F1
-:102570000E94DF20882314F0FF24F394F801F082F6
-:10258000820108561F4FFF24D20194966D917D91D0
-:102590008D919C91979720915701309158014091CE
-:1025A000590150915A010E94DF20882314F0FF2422
-:1025B000F394F801F0822EE930E0420E531ED2016E
-:1025C0006D917D918D919C91E2E6FFEF4E0E5F1E25
-:1025D00022E030E040E050E00E94FF21509540951D
-:1025E000309521953F4F4F4F5F4FD201A458BF4FB9
-:1025F0002C9311963C93119712964C931297139625
-:102600005C931397F201E058FF4F2083318342839C
-:10261000538324E830E0420E531E8D919D910D901E
-:10262000BC91A02DF20181939193A193B1938193D9
-:102630009193A193B19380839183A283B383A7E401
-:10264000B1E0232D322DC901FC0184E101900D92EE
-:102650008150E1F7A4960FB6F894DEBF0FBECDBF50
-:10266000CF91DF911F910F91FF90EF90DF90CF906E
-:10267000BF90AF909F908F907F906F905F904F90A2
-:102680003F902F9008951F93CF93DF93EC0106C0E6
-:1026900061E070E080E090E00E94B91E8091460108
-:1026A000882329F480914501833059F4F1CF20919A
-:1026B00045018091460130E090E0019728173907E5
-:1026C00039F3109145011F5F143008F010E0E12F3D
-:1026D000F0E0EE0FFF1FE35CFE4F80819181BE01B1
-:1026E0000E94D80E10934501DF91CF911F9108955C
-:1026F000CF93DF93EC019B01A1EFB1E0E7E4F1E0C0
-:1027000084E101900D928150E1F788819981AA813D
-:10271000BB818093F1019093F201A093F301B093F8
-:10272000F401F90180819181A281B381809301023A
-:1027300090930202A0930302B093040281EF91E010
-:102740000E944313DF91CF910895CF93DF93EC0163
-:102750009B01A1EFB1E0E7E4F1E084E101900D928B
-:102760008150E1F788819981AA81BB818093F5012D
-:102770009093F601A093F701B093F801F9018081DD
-:102780009181A281B3818093010290930202A09370
-:102790000302B093040281EF91E00E944313DF91A2
-:1027A000CF910895CF93DF93EC019B01A1EFB1E0AE
-:1027B000E7E4F1E084E101900D928150E1F7888136
-:1027C0009981AA81BB818093F9019093FA01A0932A
-:1027D000FB01B093FC01F90180819181A281B38159
-:1027E0008093010290930202A0930302B09304022B
-:1027F00081EF91E00E944313DF91CF9108954F92B2
-:102800005F926F927F928F929F92AF92BF92CF9280
-:10281000DF92EF92FF920F931F93DF93CF93CDB789
-:10282000DEB7C056D0400FB6F894DEBF0FBECDBFA6
-:10283000FC01AB0180818F3209F454C580E090E047
-:10284000A0E0B0E08093DD019093DE01A093DF0172
-:10285000B093E0018093E1019093E201A093E30142
-:10286000B093E4018093E5019093E601A093E70122
-:10287000B093E8018093E9019093EA01A093EB0102
-:10288000B093EC018093ED019093EE01A093EF01E2
-:10289000B093F00185E092E0BF010E9436062091DE
-:1028A000050230910602C90183709074892B91F45E
-:1028B0002115310579F0809123019091240197FD34
-:1028C00009C09093080280930702216030930602AA
-:1028D00020930502009105021091060200FFF0C34B
-:1028E0008091070290910802909324018093230124
-:1028F000ADEDB1E0E7E4F1E084E101900D928150AB
-:10290000E1F780912501980128703070882309F43F
-:1029100049C0232B81F08091110290911202A09165
-:102920001302B09114028093DD019093DE01A09315
-:10293000DF01B093E00104FF10C080911502909177
-:102940001602A0911702B09118028093E1019093B2
-:10295000E201A093E301B093E40105FF10C0809170
-:10296000190290911A02A0911B02B0911C0280934F
-:10297000E5019093E601A093E701B093E80115FF0C
-:1029800081C080911D0290911E02A0911F02B09102
-:1029900020028093E9019093EA01A093EB01B093A8
-:1029A000EC0170C0232BD1F06091DD017091DE014C
-:1029B0008091DF019091E00120911102309112028B
-:1029C00040911302509114020E942C1F6093DD016C
-:1029D0007093DE018093DF019093E00104FF1AC041
-:1029E0006091E1017091E2018091E3019091E40135
-:1029F0002091150230911602409117025091180251
-:102A00000E942C1F6093E1017093E2018093E30127
-:102A10009093E40105FF1AC06091E5017091E60111
-:102A20008091E7019091E8012091190230911A02FA
-:102A300040911B0250911C020E942C1F6093E501E3
-:102A40007093E6018093E7019093E80115FF1AC0A7
-:102A50006091E9017091EA018091EB019091EC01A4
-:102A600020911D0230911E0240911F0250912002C0
-:102A70000E942C1F6093E9017093EA018093EB019F
-:102A80009093EC0111FF10C08091290290912A02CD
-:102A9000A0912B02B0912C028093ED019093EE0156
-:102AA000A093EF01B093F00180910702909108028A
-:102AB0008130910539F18C31910549F1892B09F06B
-:102AC0001BC2E090ED01F090EE010091EF0110913A
-:102AD000F00180E090E8ABE3B5E48093ED019093E2
-:102AE000EE01A093EF01B093F0018DED91E00E9413
-:102AF0004313E092ED01F092EE010093EF01109389
-:102B0000F001F0C38DED91E00E944313EBC30F2E53
-:102B1000F0E06F2EF0E07F2EFAE78F2EF4E49F2E88
-:102B2000F02D609257017092580180925901909255
-:102B30005A0160914701709148018091490190913B
-:102B40004A0120E030E040EA50E40E942B1F6983F4
-:102B50007A838B839C830F2EF0E0EF2EF0E8FF2E1C
-:102B6000FBE30F2FF5E41F2FF02DED82FE820F8384
-:102B70001887CE010196BE016B5F7F4F0E947813CC
-:102B800060914701709148018091490190914A01FB
-:102B900020E030E04AE753E40E942B1F69877A87E0
-:102BA0008B879C87ED86FE860F87188BCE01099652
-:102BB000BE01635F7F4F0E94781320E030E040E069
-:102BC00050E02093470130934801409349015093CE
-:102BD0004A01609257017092580180925901909277
-:102BE0005A0180E090E0A0E8BFE3898B9A8BAB8B21
-:102BF000BC8B6D8A7E8A8F8A988ECE014196BE01EB
-:102C00006B5E7F4F0E94781360914701709148017D
-:102C10008091490190914A0120E030E040E251E486
-:102C20000E942B1F698F7A8F8B8F9C8F6D8E7E8E6B
-:102C30008F8E98A2CE014996BE01635E7F4F0E949F
-:102C4000781320E030E040E050E0209347013093DB
-:102C500048014093490150934A0160914B017091A2
-:102C60004C0180914D0190914E0120E030E040EA0E
-:102C700050E40E942B1F69A37AA38BA39CA3EDA20F
-:102C8000FEA20FA318A7CE018196BE016B5D7F4FF8
-:102C90000E94A51360914B0170914C0180914D01F0
-:102CA00090914E0120E030E04AE753E40E942B1F50
-:102CB00069A77AA78BA79CA7EDA6FEA60FA718ABBE
-:102CC000CE018996BE01635D7F4F0E94A51380E00F
-:102CD00090E0A0E0B0E080934B0190934C01A09372
-:102CE0004D01B0934E01609257017092580180924D
-:102CF000590190925A0120E030E040E85FE329ABAF
-:102D00003AAB4BAB5CAB6DAA7EAA8FAA98AECE0154
-:102D1000C196BE016B5C7F4F0E94A51360914B0171
-:102D200070914C0180914D0190914E0120E030E076
-:102D300040E251E40E942B1F69AF7AAF8BAF9CAF8A
-:102D4000FE01FD966082718282829382CE01C996D5
-:102D5000BF010E94A51380E090E0A0E0B0E0809366
-:102D60004B0190934C01A0934D01B0934E010F2E57
-:102D7000F0E0AF2EF0E0BF2EF0EACF2EF1E4DF2E30
-:102D8000F02DA0925701B0925801C0925901D092F3
-:102D90005A0151E4452E512C4C0E5D1E60914F019D
-:102DA00070915001809151019091520120E030E0EA
-:102DB00040E05FE30E942B1FD2016D937D938D93C2
-:102DC0009C931397FE01EB5BFF4F0F2EF0E0EF2E6D
-:102DD000F0E0FF2EF8E40F2FF2E41F2FF02DE08239
-:102DE000F18202831383C201BF010E94D21349E41E
-:102DF000442E512C4C0E5D1E60914F01709150017C
-:102E0000809151019091520120E030E04AE753E473
-:102E10000E942B1FF2016083718382839383FE01E2
-:102E2000E35BFF4FE082F18202831383C201BF01A3
-:102E30000E94D21320E030E040E050E020934F01A8
-:102E4000309350014093510150935201A092570189
-:102E5000B0925801C0925901D0925A01DE01AF5A86
-:102E6000BF4F20E030E040E85FE32D933D934D936A
-:102E70005C931397FE01EB5AFF4FA082B182C2828E
-:102E8000D382CD01BF010E94D2138E01075A1F4F7A
-:102E900060914F01709150018091510190915201C8
-:102EA00020E030E040E050E40E942B1FD8016D93F9
-:102EB0007D938D939C931397FE01E35AFF4FA0825D
-:102EC000B182C282D382C801BF010E94D21320E026
-:102ED00030E040E050E020934F01309350014093A8
-:102EE00051015093520160925701709258018092A3
-:102EF000590190925A01F6C1909146018091450185
-:102F0000981709F0DAC1E0914601F0E0EE0FFF1FDB
-:102F1000E35CFE4F0190F081E02DE855FF4F80818A
-:102F2000882309F0CAC18091070290910802853177
-:102F3000910509F467C0863191053CF48430910510
-:102F400089F0449709F0A8C024C08B35910509F495
-:102F500096C08C35910509F495C08A35910509F024
-:102F60009BC089C060910D0270910E0280910F028A
-:102F70009091100220E030E040E05FE30E942C1FBF
-:102F80000E94FC1F882777FD8095982F0E94B91E0C
-:102F900097C0ADE3B1E028EF31E24BE453E40F2EEC
-:102FA000F0E0AF2EF0E0BF2EFEEFCF2EF5E4DF2EE7
-:102FB000F02D0F2EFEE8EF2EF5E7FF2EFFE80F2F86
-:102FC000F1E41F2FF02D60E070E080E89FE3ED91C9
-:102FD000FD91208331834283538324833583468349
-:102FE0005783A086B186C286D386E486F586068797
-:102FF0001787608B718B828B938BE1E0A534BE07C2
-:1030000031F75EC0ADE3B1E02BE43AEE4FEF50E4B0
-:103010000F2EF0E0AF2EF0E0BF2EF0EACF2EF3E45B
-:10302000DF2EF02D0F2EFAE6EF2EFCEBFF2EF4E351
-:103030000F2FFFE31F2FF02D60E070E080E89FE38B
-:10304000ED91FD9120833183428353832483358323
-:1030500046835783A086B186C286D386E486F586EA
-:1030600006871787608B718B828B938BF1E0A53479
-:10307000BF0731F725C081E08093250121C0109260
-:1030800025011EC0A7E4B1E0EDEDF1E084E101907F
-:103090000D928150E1F714C08FE095E064E071E09B
-:1030A0000E947F1A4091070250910802662757FD3F
-:1030B0006095762F8FE095E02AE030E00E949D1B1E
-:1030C0008091050281FFD3C0909146018091450116
-:1030D000981709F0F9C0E0914601F0E0EE0FFF1FEC
-:1030E000E35CFE4F0190F081E02DE855FF4F8081B9
-:1030F000882309F0E9C08091090290910A028C3678
-:1031000091057CF48A3691050CF0B1C088369105A2
-:1031100089F0893691050CF040C0892B09F4A7C0CD
-:1031200092C08E37910509F44FC08F37910509F091
-:103130008AC068C0809105029091060292FF97C0F4
-:10314000E0913201F0E0EE0FFF1FE25DFE4F0081E3
-:10315000118160912D0270912E0280912F02909129
-:1031600030020E94FC1F9B01D80112967C936E9343
-:1031700011978BE090E0FC012E9FC0012F9F900DD6
-:103180003E9F900D11246AE070E00E94CA21D80190
-:1031900014967C936E9313976AC08FE095E06BE072
-:1031A00071E00E947F1AE0913201F0E0EE0FFF1F04
-:1031B000E25DFE4F808191810E94A507BC018FE0F6
-:1031C00095E00E94781B53C0E0913201F0E0EE0FD1
-:1031D000FF1FE25DFE4F0081118160910D02709131
-:1031E0000E0280910F029091100220E030E040E04A
-:1031F0005FE30E942C1F0E94FC1F9B01AC01C801D1
-:1032000061E01DC0E0913201F0E0EE0FFF1FE25DD2
-:10321000FE4F0081118160910D0270910E0280912C
-:103220000F029091100220E030E040E05FE30E9446
-:103230002C1F0E94FC1F9B01AC01C80160E0A9018A
-:103240000E94260814C08FE095E06EE071E00E94B5
-:103250007F1A4091090250910A02662757FD609536
-:10326000762F8FE095E02AE030E00E949D1B809150
-:1032700005029091060296FF35C09091460180911B
-:103280004501981741F5E0914601F0E0EE0FFF1F70
-:10329000E35CFE4F0190F081E02DE855FF4F808107
-:1032A0008823C9F490910B02911191E08091320131
-:1032B0009817C1F09093320115C061E070E080E092
-:1032C00090E00E94B91E18CE61E070E080E090E0CE
-:1032D0000E94B91EF9CE61E070E080E090E00E94AB
-:1032E000B91ECBCFC05ADF4F0FB6F894DEBF0FBE6A
-:1032F000CDBFCF91DF911F910F91FF90EF90DF90A5
-:10330000CF90BF90AF909F908F907F906F905F9085
-:103310004F9008958FE095E00E94EA1C882311F1F8
-:103320008FE095E00E94E51C809322018D3019F416
-:103330008AE0809322019091220190329CF09B338D
-:1033400019F481E08093DC018091DC01882351F441
-:103350008091DB01E82FF0E0E55AFE4F90838F5F0C
-:103360008093DB018091DB0187FF04C01092DB01B9
-:103370001092DC016091DB01662351F18091220102
-:103380008A3031F570E0FB01E55AFE4F10828BE583
-:1033900091E00E94FF131092DB011092DC018091FA
-:1033A0003301882379F08FE095E065E171E00E94B8
-:1033B0007F1A8FE095E063E371E00E94AE1A1092ED
-:1033C000330108958FE095E069E171E00E94AE1A43
-:1033D00008950E94E5070E948A190895EF92FF92CE
-:1033E0000F931F936FE6E62EF12CF70180818D7FFE
-:1033F00080830E94E3041092330182E592E090936F
-:103400002F0180932E0189E392E090933101809304
-:1034100030011092320110924501109246018BE664
-:1034200092E090933E0180933D0184E193E090937C
-:10343000400180933F018DEB93E090934201809394
-:10344000410186E694E090934401809343011092F9
-:10345000DB011092DC0180E090E0A0E0B0E080931E
-:10346000470190934801A0934901B0934A0180938A
-:103470004B0190934C01A0934D01B0934E0180936A
-:103480004F0190935001A0935101B093520180934A
-:10349000530190935401A0935501B093560180E0DD
-:1034A00090E0AAE7B4E48093570190935801A09369
-:1034B0005901B0935A010FE015E0C80140E05BE408
-:1034C00060E070E00E94EF1CC8016CE171E00E94B6
-:1034D000AE1A68EE73E080E090E00E946105F701AB
-:1034E0008081826080831F910F91FF90EF900895FB
-:1034F0000E94F11E0E94EE190E94E919FDCF0F9360
-:103500001F93CF93DF938C01EB0109C02196D80163
-:10351000ED91FC910190F081E02DC8010995688141
-:103520006623A1F7DF91CF911F910F9108950F931B
-:103530001F938C01DC01ED91FC910190F081E02D55
-:103540006DE00995D801ED91FC910190F081E02D9D
-:10355000C8016AE009951F910F9108950F931F9379
-:103560008C010E947F1AC8010E94971A1F910F9127
-:1035700008952F923F924F925F926F927F928F9217
-:103580009F92AF92BF92CF92DF92EF92FF920F93F2
-:103590001F93DF93CF93CDB7DEB7A0970FB6F89404
-:1035A000DEBF0FBECDBF1C016A017B014115510575
-:1035B0006105710549F4DC01ED91FC910190F08108
-:1035C000E02D60E3099554C0882499245401422ECB
-:1035D00055246624772401E010E00C0F1D1F080D10
-:1035E000191DC701B601A30192010E94DD21F80156
-:1035F00060830894811C911CA11CB11CC701B601F9
-:10360000A30192010E94DD21C901DA016C017D0153
-:10361000C114D104E104F104F1F681E0E82EF12CAB
-:10362000EC0EFD1EE80CF91C3E010894611C711C97
-:10363000D501C4010197A109B1096C01C818D908C5
-:1036400014C0F601EE0DFF1D60816A3010F4605D5C
-:1036500001C0695CD101ED91FC910190F081E02DF8
-:10366000C10109950894E108F1086E147F0449F737
-:10367000A0960FB6F894DEBF0FBECDBFCF91DF91FD
-:103680001F910F91FF90EF90DF90CF90BF90AF9080
-:103690009F908F907F906F905F904F903F902F9072
-:1036A0000895EF92FF920F931F93CF93DF93EC0156
-:1036B0007A018B0177FF0FC0E881F9810190F081D9
-:1036C000E02D6DE2099510950095F094E094E11CD1
-:1036D000F11C011D111DCE01B801A7012AE00E94B5
-:1036E000B91ADF91CF911F910F91FF90EF9008953C
-:1036F0000F931F938C01AB01662757FD6095762FC2
-:103700000E94511BC8010E94971A1F910F910895A2
-:10371000DC012115310541F4ED91FC910190F0811E
-:10372000E02D642F099508952A30310519F40E947F
-:10373000511B08950E94B91A08950F931F938C018D
-:103740000E94881BC8010E94971A1F910F9108952B
-:10375000909126019295990F990F907C8770892BF3
-:1037600080937C0080917A00806480937A008091BD
-:103770007A0086FDFCCF2091780040917900942F4B
-:1037800080E030E0282B392BC90108951F93CF9397
-:10379000DF93182FEB0161E00E941B1CE12FF0E08A
-:1037A000E553FF4F8491833051F48091800080680D
-:1037B00080938000D0938900C093880038C0843003
-:1037C00051F480918000806280938000D0938B00C0
-:1037D000C0938A002CC0813029F484B5806884BDF0
-:1037E000C7BD25C0823029F484B5806284BDC8BDC0
-:1037F0001EC0863041F48091B00080688093B00094
-:10380000C093B30014C0873041F48091B00080624F
-:103810008093B000C093B4000AC0C038D1051CF436
-:10382000812F60E002C0812F61E00E94391CDF918E
-:10383000CF911F910895282F30E0C90185559F4FE2
-:10384000FC01949125573F4FF9018491882381F021
-:10385000E82FF0E0E458FF4FE491F0E0662329F40C
-:10386000808190958923808308958081892B80832E
-:103870000895482F50E0CA0185539F4FFC012491C1
-:10388000CA0185559F4FFC01949145575F4FFA013E
-:1038900034913323C1F1222331F1213019F484B55D
-:1038A0008F7704C0223021F484B58F7D84BD1BC086
-:1038B000233021F4809180008F7705C0243031F4CB
-:1038C000809180008F7D809380000DC0263021F490
-:1038D0008091B0008F7705C0273029F48091B00027
-:1038E0008F7D8093B000E32FF0E0EF57FF4FE4911E
-:1038F000F0E0662329F480819095892380830895E0
-:103900008081892B80830895482F50E0CA01855318
-:103910009F4FFC012491CA0185559F4FFC01949152
-:1039200045575F4FFA013491332319F420E030E01A
-:1039300035C0222331F1213019F484B58F7704C0CA
-:10394000223021F484B58F7D84BD1BC0233021F447
-:10395000809180008F7705C0243031F48091800001
-:103960008F7D809380000DC0263021F48091B000BF
-:103970008F7705C0273029F48091B0008F7D809328
-:10398000B000E32FF0E0EA57FF4FE491F0E08081D0
-:1039900020E030E0892311F021E030E0C9010895F2
-:1039A0008BE291E09093100580930F051092110522
-:1039B000909313058093120581E080931405089578
-:1039C000FC0182810E947C1D0895FC0182810E947D
-:1039D000A21D0895FC0182810E948C1D0895FC01A6
-:1039E00082810E94F41C0895EF92FF920F931F931F
-:1039F0007A018B018823F1F5CB01BA0122E030E096
-:103A000040E050E00E94FF21205C3D4B404F5F4F63
-:103A1000CA01B901A80197010E94FF21C901DA0179
-:103A20000197A109B109292F3A2F4B2F552747FD9F
-:103A30005A950196A11DB11D2093C5008150809318
-:103A4000C4001092C0001092C1001092C200809178
-:103A5000C10080618093C1008091C1008860809323
-:103A6000C1008091C10080688093C10086E080938E
-:103A7000C2003DC0CB01BA0122E030E040E050E09E
-:103A80000E94FF21205C3D4B404F5F4FCA01B901AE
-:103A9000A80197010E94FF21C901DA010197A1093C
-:103AA000B109292F3A2F4B2F552747FD5A950196DB
-:103AB000A11DB11D2093CD0081508093CC001092A8
-:103AC000C8001092C9001092CA008091C90080619C
-:103AD0008093C9008091C90088608093C90080915B
-:103AE000C90080688093C90086E08093CA001F9156
-:103AF0000F91FF90EF900895882339F48091C000D2
-:103B000085FFFCCF6093C60008958091C80085FFB3
-:103B1000FCCF6093CE000895E82FF0E0EE0FFF1F7A
-:103B2000DF01AB5EBA4F8D919C9180589F4FE75E4D
-:103B3000FA4F20813181821B930B60E870E00E9474
-:103B4000CA210895CF93DF93A82FB0E0FD01EE0FB7
-:103B5000FF1FEF01C75EDA4F48815981EB5EFA4FD4
-:103B6000808191818417950719F42FEF3FEF13C0DF
-:103B7000B695BA2FAA27B795A795A40FB51FA75D2D
-:103B8000BA4F2C91CA01019660E870E00E94CA21E8
-:103B90009983888330E0C901DF91CF9108951F9206
-:103BA0000F920FB60F9211242F933F934F935F9371
-:103BB0006F937F938F939F93AF93BF93EF93FF93F5
-:103BC0004091C600E0911505F0911605CF010196D0
-:103BD00060E870E00E94CA219C01809119059091D3
-:103BE0001A052817390739F0E75DFA4F40833093FB
-:103BF000160520931505FF91EF91BF91AF919F910D
-:103C00008F917F916F915F914F913F912F910F9085
-:103C10000FBE0F901F9018951F920F920FB60F9224
-:103C200011242F933F934F935F936F937F938F93C1
-:103C30009F93AF93BF93EF93FF934091CE00E0919A
-:103C40001705F0911805CF01019660E870E00E9419
-:103C5000CA219C0180911B0590911C0528173907EA
-:103C600039F0E755FA4F4083309318052093170534
-:103C7000FF91EF91BF91AF919F918F917F916F9144
-:103C80005F914F913F912F910F900FBE0F901F901A
-:103C900018951F920F920FB60F9211242F938F93A6
-:103CA0009F93AF93BF9380911D0590911E05A091A6
-:103CB0001F05B09120050196A11DB11D80931D0522
-:103CC00090931E05A0931F05B093200580912105B8
-:103CD00090912205A0912305B09124058050904C2D
-:103CE000AF4FBF4F8093210590932205A0932305EA
-:103CF000B093240527C08091210590912205A091C1
-:103D00002305B091240580589E43A040B040809385
-:103D1000210590932205A0932305B093240580915B
-:103D2000250590912605A0912705B09128050196BB
-:103D3000A11DB11D8093250590932605A09327050D
-:103D4000B09328058091210590912205A09123052B
-:103D5000B091240581589E43A040B04060F6BF91C9
-:103D6000AF919F918F912F910F900FBE0F901F9049
-:103D70001895EF92FF920F931F937B018C018FB7E1
-:103D8000F894409125055091260560912705709182
-:103D900028058FBF2FB7F8948091250590912605AF
-:103DA000A0912705B09128052FBF841B950BA60B6A
-:103DB000B70BE816F9060A071B0760F71F910F916A
-:103DC000FF90EF900895019759F0FC01EE0FFF1F4F
-:103DD000EE0FFF1F32978FB7F8943197F1F78FBF2F
-:103DE0000895789484B5826084BD84B5816084BD73
-:103DF00085B5826085BD85B5816085BDEEE6F0E064
-:103E0000808181608083E1E8F0E08081826080834E
-:103E1000808181608083E0E8F0E080818160808340
-:103E2000E1EBF0E0808184608083E0EBF0E0808172
-:103E300081608083EAE7F0E0808184608083808114
-:103E400082608083808181608083808180688083BC
-:103E50001092C10008955058BB27AA270ED075C1F3
-:103E600066D130F06BD120F031F49F3F11F41EF495
-:103E70005BC10EF4E095E7FB51C1E92F77D180F3E8
-:103E8000BA17620773078407950718F071F49EF557
-:103E90008FC10EF4E0950B2EBA2FA02D0B01B901A6
-:103EA00090010C01CA01A0011124FF27591B99F0B0
-:103EB000593F50F4503E68F11A16F040A22F232FBC
-:103EC000342F4427585FF3CF469537952795A7950C
-:103ED000F0405395C9F77EF41F16BA0B620B730BB3
-:103EE000840BBAF09150A1F0FF0FBB1F661F771F24
-:103EF000881FC2F70EC0BA0F621F731F841F48F4D9
-:103F0000879577956795B795F7959E3F08F0B3CF5E
-:103F10009395880F08F09927EE0F97958795089548
-:103F2000D9D008F481E008950CD00FC107D140F03A
-:103F3000FED030F021F45F3F19F0F0C0511139C1CB
-:103F4000F3C014D198F39923C9F35523B1F3951B0A
-:103F5000550BBB27AA2762177307840738F09F5FAA
-:103F60005F4F220F331F441FAA1FA9F333D00E2E19
-:103F70003AF0E0E830D091505040E695001CCAF786
-:103F800029D0FE2F27D0660F771F881FBB1F26174B
-:103F900037074807AB07B0E809F0BB0B802DBF011E
-:103FA000FF2793585F4F2AF09E3F510568F0B6C037
-:103FB00000C15F3FECF3983EDCF3869577956795FB
-:103FC000B795F7959F5FC9F7880F911D96958795CF
-:103FD00097F90895E1E0660F771F881FBB1F6217EE
-:103FE00073078407BA0720F0621B730B840BBA0BAC
-:103FF000EE1F88F7E095089504D06894B111D9C0F8
-:104000000895BCD088F09F5790F0B92F9927B751E9
-:10401000A0F0D1F0660F771F881F991F1AF0BA958C
-:10402000C9F712C0B13081F0C3D0B1E00895C0C06B
-:10403000672F782F8827B85F39F0B93FCCF3869582
-:1040400077956795B395D9F73EF4909580957095DF
-:1040500061957F4F8F4F9F4F0895E89409C097FB5C
-:104060003EF490958095709561957F4F8F4F9F4F4F
-:104070009923A9F0F92F96E9BB279395F695879593
-:1040800077956795B795F111F8CFFAF4BB0F11F456
-:1040900060FF1BC06F5F7F4F8F4F9F4F16C08823FD
-:1040A00011F096E911C0772321F09EE8872F762F33
-:1040B00005C0662371F096E8862F70E060E02AF074
-:1040C0009A95660F771F881FDAF7880F9695879560
-:1040D00097F90895990F0008550FAA0BE0E8FEEF35
-:1040E00016161706E807F907C0F012161306E407BC
-:1040F000F50798F0621B730B840B950B39F40A26B5
-:1041000061F0232B242B252B21F408950A2609F492
-:10411000A140A6958FEF811D811D089597F99F6796
-:1041200080E870E060E008959FEF80EC089500243F
-:104130000A941616170618060906089500240A940C
-:1041400012161306140605060895092E0394000C92
-:1041500011F4882352F0BB0F40F4BF2B11F460FF21
-:1041600004C06F5F7F4F8F4F9F4F089557FD90584A
-:10417000440F551F59F05F3F71F04795880F97FB2B
-:10418000991F61F09F3F79F08795089512161306E5
-:104190001406551FF2CF4695F1DF08C01616170614
-:1041A0001806991FF1CF8695710561050894089549
-:1041B000E894BB2766277727CB0197F908958ADF14
-:1041C00008F48FEF08950BD0C0CFB1DF28F0B6DF31
-:1041D00018F0952309F0A2CFA7CF1124EACFC6DFAC
-:1041E000A0F3959FD1F3950F50E0551F629FF0010A
-:1041F000729FBB27F00DB11D639FAA27F00DB11D63
-:10420000AA1F649F6627B00DA11D661F829F2227EB
-:10421000B00DA11D621F739FB00DA11D621F839F72
-:10422000A00D611D221F749F3327A00D611D231F48
-:10423000849F600D211D822F762F6A2F11249F5796
-:1042400050408AF0E1F088234AF0EE0FFF1FBB1FB9
-:10425000661F771F881F91505040A9F79E3F510558
-:1042600070F05CCFA6CF5F3FECF3983EDCF3869511
-:1042700077956795B795F795E7959F5FC1F7FE2B03
-:10428000880F911D9695879597F9089511F40EF46E
-:104290004BCF4AC073DFD0F39923D9F3CEF39F57A6
-:1042A000550B87FF51D056959795B0E020F4660FD7
-:1042B000771F881FBB1F1F930F9300249001A0013D
-:1042C0008001F001A0E80E0F1F1F0A1E511D601B88
-:1042D000710B8009B50B48F4600F711F801DB51F6D
-:1042E0000E1B1F0B0A0A510907C02E0F3F1F4A1F42
-:1042F0000E0F1F1F0A1E511D660F771F881FBB1F41
-:10430000A695F795E795F8F60617170708065B07D1
-:10431000211D311D411D0F911F91B901842F91580D
-:10432000880F9695879508959F3F49F0915028F4FE
-:10433000869577956795B7959F5F80389F4F880FD3
-:104340009695879597F9089591505040660F771F7D
-:10435000881FD2F70895629FD001739FF001829F5A
-:10436000E00DF11D649FE00DF11D929FF00D839F04
-:10437000F00D749FF00D659FF00D9927729FB00DA1
-:10438000E11DF91F639FB00DE11DF91FBD01CF01B4
-:104390001124089597FB092E07260AD077FD04D033
-:1043A00049D006D000201AF4709561957F4F08958A
-:1043B000F6F7909581959F4F0895A1E21A2EAA1BBA
-:1043C000BB1BFD010DC0AA1FBB1FEE1FFF1FA217C5
-:1043D000B307E407F50720F0A21BB30BE40BF50BC2
-:1043E000661F771F881F991F1A9469F7609570954B
-:1043F000809590959B01AC01BD01CF01089597FB7D
-:10440000092E05260ED057FD04D0D7DF0AD0001C98
-:1044100038F450954095309521953F4F4F4F5F4F61
-:104420000895F6F790958095709561957F4F8F4F21
-:104430009F4F0895AA1BBB1B51E107C0AA1FBB1FBA
-:10444000A617B70710F0A61BB70B881F991F5A9520
-:10445000A9F780959095BC01CD010895EE0FFF1F3F
-:104460000590F491E02D0994911106C0803219F065
-:1044700089508550D0F708959927882708952F925D
-:104480003F924F925F926F927F928F929F92AF92E4
-:10449000BF92CF92DF92EF92FF920F931F93CF9331
-:1044A000DF93CDB7DEB7CA1BDB0B0FB6F894DEBFC8
-:1044B0000FBECDBF09942A88398848885F846E84EE
-:1044C0007D848C849B84AA84B984C884DF80EE8038
-:1044D000FD800C811B81AA81B981CE0FD11D0FB641
-:1044E000F894DEBF0FBECDBFED010895F894FFCF65
-:1044F000453A20006875683F204700543A004875E7
-:10450000683F204D006F6B20006F6B007374617209
-:0E45100074003FFFFF010100000000E01C00EE
-:00000001FF
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.pde
deleted file mode 100644
index 15b5311d..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/FiveD_GCode_Interpreter.pde
+++ /dev/null
@@ -1,353 +0,0 @@
-// Yep, this is actually -*- c++ -*-
-
-// Sanguino G-code Interpreter
-// Arduino v1.0 by Mike Ellery - initial software (mellery@gmail.com)
-// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)
-// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)
-// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
-// Sanguino v1.4 by Adrian Bowyer - added the Sanguino; extensive mods... (a.bowyer@bath.ac.uk)
-// Sanguino v1.5 by Adrian Bowyer - implemented 4D Bressenham XYZ+ stepper control... (a.bowyer@bath.ac.uk)
-// Sanguino v1.6 by Adrian Bowyer - implemented RS485 extruders
-
-#ifndef __AVR_ATmega644P__
-#error Oops! Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu.
-#endif
-
-#include <ctype.h>
-#include <HardwareSerial.h>
-#include "WProgram.h"
-#include "vectors.h"
-#include "parameters.h"
-#include "intercom.h"
-#include "pins.h"
-#include "extruder.h"
-#include "cartesian_dda.h"
-
-// Maintain a list of extruders...
-
-extruder* ex[EXTRUDER_COUNT];
-byte extruder_in_use = 0;
-
-// Text placed in this (terminated with 0) will be transmitted back to the host
-// along with the next G Code acknowledgement.
-char debugstring[10];
-
-#if MOTHERBOARD < 2
-
-// TODO: For some reason, if you declare the following two in the order ex0 ex1 then
-// ex0 won't drive its stepper. They seem fine this way round though. But that's got
-// to be a bug.
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(EXTRUDER_1_MOTOR_DIR_PIN, EXTRUDER_1_MOTOR_SPEED_PIN , EXTRUDER_1_HEATER_PIN,
- EXTRUDER_1_FAN_PIN, EXTRUDER_1_TEMPERATURE_PIN, EXTRUDER_1_VALVE_DIR_PIN,
- EXTRUDER_1_VALVE_ENABLE_PIN, EXTRUDER_1_STEP_ENABLE_PIN);
-#endif
-
-static extruder ex0(EXTRUDER_0_MOTOR_DIR_PIN, EXTRUDER_0_MOTOR_SPEED_PIN , EXTRUDER_0_HEATER_PIN,
- EXTRUDER_0_FAN_PIN, EXTRUDER_0_TEMPERATURE_PIN, EXTRUDER_0_VALVE_DIR_PIN,
- EXTRUDER_0_VALVE_ENABLE_PIN, EXTRUDER_0_STEP_ENABLE_PIN);
-
-
-#else
-
-#if EXTRUDER_COUNT == 2
-static extruder ex1(2);
-#endif
-
-static extruder ex0(1);
-
-intercom talker;
-
-#endif
-
-// Each entry in the buffer is an instance of cartesian_dda.
-
-cartesian_dda* cdda[BUFFER_SIZE];
-
-static cartesian_dda cdda0;
-static cartesian_dda cdda1;
-static cartesian_dda cdda2;
-static cartesian_dda cdda3;
-
-volatile byte head;
-volatile byte tail;
-
-// Where the machine is from the point of view of the command stream
-
-FloatPoint where_i_am;
-
-// Make sure each DDA knows which extruder to use
-
-//inline void setExtruder()
-//{
-// for(byte i = 0; i < BUFFER_SIZE; i++)
-// cdda[i]->set_extruder(ex[extruder_in_use]);
-//}
-
-
-// Our interrupt function
-
-SIGNAL(SIG_OUTPUT_COMPARE1A)
-{
- disableTimerInterrupt();
-
- if(cdda[tail]->active())
- cdda[tail]->dda_step();
- else
- dQMove();
-
- enableTimerInterrupt();
-}
-
-void setup()
-{
- disableTimerInterrupt();
- setupTimerInterrupt();
- debugstring[0] = 0;
-
- ex[0] = &ex0;
-#if EXTRUDER_COUNT == 2
- ex[1] = &ex1;
-#endif
- extruder_in_use = 0;
-
- head = 0;
- tail = 0;
-
- cdda[0] = &cdda0;
- cdda[1] = &cdda1;
- cdda[2] = &cdda2;
- cdda[3] = &cdda3;
-
- //setExtruder();
-
- init_process_string();
-
- where_i_am.x = 0.0;
- where_i_am.y = 0.0;
- where_i_am.z = 0.0;
- where_i_am.e = 0.0;
- where_i_am.f = SLOW_XY_FEEDRATE;
-
- Serial.begin(19200);
- Serial.println("start");
-
- setTimer(DEFAULT_TICK);
- enableTimerInterrupt();
-}
-
-void loop()
-{
- manage_all_extruders();
- get_and_do_command();
-}
-
-//******************************************************************************************
-
-// The move buffer
-
-inline bool qFull()
-{
- if(tail == 0)
- return head == (BUFFER_SIZE - 1);
- else
- return head == (tail - 1);
-}
-
-inline bool qEmpty()
-{
- return tail == head && !cdda[tail]->active();
-}
-
-inline void qMove(const FloatPoint& p)
-{
- while(qFull()) delay(WAITING_DELAY);
- byte h = head;
- h++;
- if(h >= BUFFER_SIZE)
- h = 0;
- cdda[h]->set_target(p);
- head = h;
-}
-
-inline void dQMove()
-{
- if(qEmpty())
- return;
- byte t = tail;
- t++;
- if(t >= BUFFER_SIZE)
- t = 0;
- cdda[t]->dda_start();
- tail = t;
-}
-
-inline void setUnits(bool u)
-{
- for(byte i = 0; i < BUFFER_SIZE; i++)
- cdda[i]->set_units(u);
-}
-
-
-inline void setPosition(const FloatPoint& p)
-{
- where_i_am = p;
-}
-
-
-//******************************************************************************************
-
-// Interrupt functions
-
-void setupTimerInterrupt()
-{
- //clear the registers
- TCCR1A = 0;
- TCCR1B = 0;
- TCCR1C = 0;
- TIMSK1 = 0;
-
- //waveform generation = 0100 = CTC
- TCCR1B &= ~(1<<WGM13);
- TCCR1B |= (1<<WGM12);
- TCCR1A &= ~(1<<WGM11);
- TCCR1A &= ~(1<<WGM10);
-
- //output mode = 00 (disconnected)
- TCCR1A &= ~(1<<COM1A1);
- TCCR1A &= ~(1<<COM1A0);
- TCCR1A &= ~(1<<COM1B1);
- TCCR1A &= ~(1<<COM1B0);
-
- //start off with a slow frequency.
- setTimerResolution(4);
- setTimerCeiling(65535);
-}
-
-void setTimerResolution(byte r)
-{
- //here's how you figure out the tick size:
- // 1000000 / ((16000000 / prescaler))
- // 1000000 = microseconds in 1 second
- // 16000000 = cycles in 1 second
- // prescaler = your prescaler
-
- // no prescaler == 0.0625 usec tick
- if (r == 0)
- {
- // 001 = clk/1
- TCCR1B &= ~(1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /8 == 0.5 usec tick
- else if (r == 1)
- {
- // 010 = clk/8
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /64 == 4 usec tick
- else if (r == 2)
- {
- // 011 = clk/64
- TCCR1B &= ~(1<<CS12);
- TCCR1B |= (1<<CS11);
- TCCR1B |= (1<<CS10);
- }
- // prescale of /256 == 16 usec tick
- else if (r == 3)
- {
- // 100 = clk/256
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B &= ~(1<<CS10);
- }
- // prescale of /1024 == 64 usec tick
- else
- {
- // 101 = clk/1024
- TCCR1B |= (1<<CS12);
- TCCR1B &= ~(1<<CS11);
- TCCR1B |= (1<<CS10);
- }
-}
-
-unsigned int getTimerCeiling(const long& delay)
-{
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs)
- if (delay <= 65535L)
- return (delay & 0xffff);
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs)
- else if (delay <= 524280L)
- return ((delay / 8) & 0xffff);
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs)
- else if (delay <= 4194240L)
- return ((delay / 64) & 0xffff);
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs)
- else if (delay <= 16776960L)
- return ((delay / 256) & 0xffff);
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs)
- else if (delay <= 67107840L)
- return ((delay / 1024) & 0xffff);
- //its really slow... hopefully we can just get by with super slow.
- else
- return 65535;
-}
-
-byte getTimerResolution(const long& delay)
-{
- // these also represent frequency: 1000000 / delay / 2 = frequency in hz.
-
- // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs (4 millisecond max))
- // range: 8Mhz max - 122hz min
- if (delay <= 65535L)
- return 0;
- // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs (32 millisecond max))
- // range:1Mhz max - 15.26hz min
- else if (delay <= 524280L)
- return 1;
- // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs (0.26 seconds max))
- // range: 125Khz max - 1.9hz min
- else if (delay <= 4194240L)
- return 2;
- // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs (1.04 seconds max))
- // range: 31.25Khz max - 0.475hz min
- else if (delay <= 16776960L)
- return 3;
- // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max))
- // range: 7.812Khz max - 0.119hz min
- else if (delay <= 67107840L)
- return 4;
- //its really slow... hopefully we can just get by with super slow.
- else
- return 4;
-}
-
-
-// Depending on how much work the interrupt function has to do, this is
-// pretty accurate between 10 us and 0.1 s. At fast speeds, the time
-// taken in the interrupt function becomes significant, of course.
-
-// Note - it is up to the user to call enableTimerInterrupt() after a call
-// to this function.
-
-inline void setTimer(long delay)
-{
- // delay is the delay between steps in microsecond ticks.
- //
- // we break it into 5 different resolutions based on the delay.
- // then we set the resolution based on the size of the delay.
- // we also then calculate the timer ceiling required. (ie what the counter counts to)
- // the result is the timer counts up to the appropriate time and then fires an interrupt.
-
- // Actual ticks are 0.0625 us, so multiply delay by 16
-
- delay <<= 4;
-
- setTimerCeiling(getTimerCeiling(delay));
- setTimerResolution(getTimerResolution(delay));
-}
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/ThermistorTable.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/ThermistorTable.h
deleted file mode 100644
index c55b9c91..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/ThermistorTable.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef THERMISTORTABLE_H_
-#define THERMISTORTABLE_H_
-
-#if MOTHERBOARD < 2
-
-// Uncomment the next line if you are using a thermistor; leave it if you have a thermocouple
-//#define USE_THERMISTOR
-
-// How many temperature samples to take for an average. each sample takes about 100 usecs.
-#define TEMPERATURE_SAMPLES 3
-
-// How accurately do we maintain the temperature?
-#define HALF_DEAD_ZONE 5
-
-// Thermistor lookup table for RepRap Temperature Sensor Boards (http://make.rrrf.org/ts)
-// See this page:
-// http://dev.www.reprap.org/bin/view/Main/Thermistor
-// for details of what goes in this table.
-// Made with createTemperatureLookup.py (http://svn.reprap.org/trunk/reprap/firmware/Arduino/utilities/createTemperatureLookup.py)
-// ./createTemperatureLookup.py --r0=100000 --t0=25 --r1=0 --r2=4700 --beta=4066 --max-adc=1023
-// r0: 100000
-// t0: 25
-// r1: 0
-// r2: 4700
-// beta: 4066
-// max adc: 1023
-#ifdef USE_THERMISTOR
-#define NUMTEMPS 20
-short temptable[NUMTEMPS][2] = {
- {1, 841},
- {54, 255},
- {107, 209},
- {160, 184},
- {213, 166},
- {266, 153},
- {319, 142},
- {372, 132},
- {425, 124},
- {478, 116},
- {531, 108},
- {584, 101},
- {637, 93},
- {690, 86},
- {743, 78},
- {796, 70},
- {849, 61},
- {902, 50},
- {955, 34},
- {1008, 3}
-};
-
-#endif
-#endif
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.h
deleted file mode 100644
index bf6e9217..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * This class controls the movement of the RepRap machine.
- * It implements a DDA in four dimensions, so the length of extruded
- * filament is treated as a variable, just like X, Y, and Z.
- *
- * Adrian Bowyer 9 May 2009
- */
-
-#ifndef CARTESIAN_DDA_H
-#define CARTESIAN_DDA_H
-
-// Main class for moving the RepRap machine about
-
-class cartesian_dda
-{
-private:
-
- //extruder* ext; // The extruder I'm currently using - keep this up to date...
-
- FloatPoint units; // Factors for converting either mm or inches to steps
-
- FloatPoint target_position; // Where it's going
- FloatPoint delta_position; // The difference between the two
- float distance; // How long the path is
-
- LongPoint current_steps; // Similar information as above in steps rather than units
- LongPoint target_steps;
- LongPoint delta_steps;
- LongPoint dda_counter; // DDA error-accumulation variables
- long t_scale; // When doing lots of t steps, scale them so the DDA doesn't spend for ever on them
-
- byte x_direction; // Am I going in the + or - direction?
- byte y_direction;
- byte z_direction;
- byte e_direction;
- byte f_direction;
-
- bool x_can_step; // Am I not at an endstop? Have I not reached the target? etc.
- bool y_can_step;
- bool z_can_step;
- bool e_can_step;
- bool f_can_step;
-
-// Variables for acceleration calculations
-
- long total_steps; // The number of steps to take along the longest movement axis
-
- long timestep; // microseconds
- bool nullmove; // this move is zero length
- bool real_move; // Flag to know if we've changed something physical
- volatile bool live; // Flag for when we're plotting a line
-
-// Internal functions that need not concern the user
-
- // Take a single step
-
- void do_x_step();
- void do_y_step();
- void do_z_step();
- void do_e_step();
-
- // Can this axis step?
-
- bool can_step(byte min_pin, byte max_pin, long current, long target, byte dir);
-
- // Read a limit switch
-
- bool read_switch(byte pin);
-
- // Work out the number of microseconds between steps
-
- long calculate_feedrate_delay(const float& feedrate);
-
- // Switch the steppers on and off
-
- void enable_steppers();
- void disable_steppers();
-
- // Custom short delay function (microseconds)
-
- //void delayMicrosecondsInterruptible(unsigned int us);
-
-
-public:
-
- cartesian_dda();
-
- // Set where I'm going
-
- void set_target(const FloatPoint& p);
-
- // Start the DDA
-
- void dda_start();
-
- // Do one step of the DDA
-
- void dda_step();
-
- // Are we running at the moment?
-
- bool active();
-
- // True for mm; false for inches
-
- void set_units(bool using_mm);
-
- // Record the selection of a new extruder
-
- //void set_extruder(extruder* ex);
-};
-
-// Short functions inline to save memory; particularly useful in the Arduino
-
-
-//inline void cartesian_dda::set_extruder(extruder* ex)
-//{
-// ext = ex;
-//}
-
-inline bool cartesian_dda::active()
-{
- return live;
-}
-
-inline void cartesian_dda::do_x_step()
-{
- digitalWrite(X_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(X_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_y_step()
-{
- digitalWrite(Y_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Y_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_z_step()
-{
- digitalWrite(Z_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Z_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_e_step()
-{
- ex[extruder_in_use]->step();
-}
-
-inline long cartesian_dda::calculate_feedrate_delay(const float& feedrate)
-{
-
- // Calculate delay between steps in microseconds. Here it is in English:
- // (feedrate is in mm/minute, distance is in mm)
- // 60000000.0*distance/feedrate = move duration in microseconds
- // move duration/total_steps = time between steps for master axis.
-
- return round( (distance*60000000.0) / (feedrate*(float)total_steps) );
-}
-
-inline bool cartesian_dda::read_switch(byte pin)
-{
- //dual read as crude debounce
-
- #if ENDSTOPS_INVERTING == 1
- return !digitalRead(pin) && !digitalRead(pin);
- #else
- return digitalRead(pin) && digitalRead(pin);
- #endif
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.pde
deleted file mode 100644
index 9f03047e..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/cartesian_dda.pde
+++ /dev/null
@@ -1,468 +0,0 @@
-#include <stdio.h>
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-
-// Initialise X, Y and Z. The extruder is initialized
-// separately.
-
-cartesian_dda::cartesian_dda()
-{
- live = false;
- nullmove = false;
-
- // Default is going forward
-
- x_direction = 1;
- y_direction = 1;
- z_direction = 1;
- e_direction = 1;
- f_direction = 1;
-
- // Default to the origin and not going anywhere
-
- target_position.x = 0.0;
- target_position.y = 0.0;
- target_position.z = 0.0;
- target_position.e = 0.0;
- target_position.f = SLOW_XY_FEEDRATE;
-
- // Set up the pin directions
-
- pinMode(X_STEP_PIN, OUTPUT);
- pinMode(X_DIR_PIN, OUTPUT);
-
- pinMode(Y_STEP_PIN, OUTPUT);
- pinMode(Y_DIR_PIN, OUTPUT);
-
- pinMode(Z_STEP_PIN, OUTPUT);
- pinMode(Z_DIR_PIN, OUTPUT);
-
-#if MOTHERBOARD > 0
- pinMode(X_ENABLE_PIN, OUTPUT);
- pinMode(Y_ENABLE_PIN, OUTPUT);
- pinMode(Z_ENABLE_PIN, OUTPUT);
-#endif
-
- //turn the motors off at the start.
-
- disable_steppers();
-
-#if ENDSTOPS_MIN_ENABLED == 1
- pinMode(X_MIN_PIN, INPUT);
- pinMode(Y_MIN_PIN, INPUT);
- pinMode(Z_MIN_PIN, INPUT);
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
- pinMode(X_MAX_PIN, INPUT);
- pinMode(Y_MAX_PIN, INPUT);
- pinMode(Z_MAX_PIN, INPUT);
-#endif
-
- // Default units are mm
-
- set_units(true);
-}
-
-// Switch between mm and inches
-
-void cartesian_dda::set_units(bool using_mm)
-{
- if(using_mm)
- {
- units.x = X_STEPS_PER_MM;
- units.y = Y_STEPS_PER_MM;
- units.z = Z_STEPS_PER_MM;
- units.e = E_STEPS_PER_MM;
- units.f = 1.0;
- } else
- {
- units.x = X_STEPS_PER_INCH;
- units.y = Y_STEPS_PER_INCH;
- units.z = Z_STEPS_PER_INCH;
- units.e = E_STEPS_PER_INCH;
- units.f = 1.0;
- }
-}
-
-
-void cartesian_dda::set_target(const FloatPoint& p)
-{
- target_position = p;
- nullmove = false;
-
- //figure our deltas.
-
- delta_position = fabsv(target_position - where_i_am);
-
- // The feedrate values refer to distance in (X, Y, Z) space, so ignore e and f
- // values unless they're the only thing there.
-
- FloatPoint squares = delta_position*delta_position;
- distance = squares.x + squares.y + squares.z;
- // If we are 0, only thing changing is e
- if(distance <= 0.0)
- distance = squares.e;
- // If we are still 0, only thing changing is f
- if(distance <= 0.0)
- distance = squares.f;
- distance = sqrt(distance);
-
- //set our steps current, target, and delta
-
- current_steps = to_steps(units, where_i_am);
- target_steps = to_steps(units, target_position);
- delta_steps = absv(target_steps - current_steps);
-
- // find the dominant axis.
- // NB we ignore the f values here, as it takes no time to take a step in time :-)
-
- total_steps = max(delta_steps.x, delta_steps.y);
- total_steps = max(total_steps, delta_steps.z);
- total_steps = max(total_steps, delta_steps.e);
-
- // If we're not going anywhere, flag the fact
-
- if(total_steps == 0)
- {
- nullmove = true;
- where_i_am = p;
- return;
- }
-
-#ifndef ACCELERATION_ON
- current_steps.f = round(target_position.f);
-#endif
-
- delta_steps.f = abs(target_steps.f - current_steps.f);
-
- // Rescale the feedrate so it doesn't take lots of steps to do
-
- t_scale = 1;
- if(delta_steps.f > total_steps)
- {
- t_scale = delta_steps.f/total_steps;
- if(t_scale >= 3)
- {
- target_steps.f = target_steps.f/t_scale;
- current_steps.f = current_steps.f/t_scale;
- delta_steps.f = abs(target_steps.f - current_steps.f);
- if(delta_steps.f > total_steps)
- total_steps = delta_steps.f;
- } else
- {
- t_scale = 1;
- total_steps = delta_steps.f;
- }
- }
-
- //what is our direction?
-
- x_direction = (target_position.x >= where_i_am.x);
- y_direction = (target_position.y >= where_i_am.y);
- z_direction = (target_position.z >= where_i_am.z);
- e_direction = (target_position.e >= where_i_am.e);
- f_direction = (target_position.f >= where_i_am.f);
-
- dda_counter.x = -total_steps/2;
- dda_counter.y = dda_counter.x;
- dda_counter.z = dda_counter.x;
- dda_counter.e = dda_counter.x;
- dda_counter.f = dda_counter.x;
-
- where_i_am = p;
-
- return;
-}
-
-
-
-void cartesian_dda::dda_step()
-{
- if(!live)
- return;
-
- do
- {
- x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
- y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
- z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
- e_can_step = can_step(-1, -1, current_steps.e, target_steps.e, e_direction);
- f_can_step = can_step(-1, -1, current_steps.f, target_steps.f, f_direction);
-
- real_move = false;
-
- if (x_can_step)
- {
- dda_counter.x += delta_steps.x;
-
- if (dda_counter.x > 0)
- {
- do_x_step();
- real_move = true;
- dda_counter.x -= total_steps;
-
- if (x_direction)
- current_steps.x++;
- else
- current_steps.x--;
- }
- }
-
- if (y_can_step)
- {
- dda_counter.y += delta_steps.y;
-
- if (dda_counter.y > 0)
- {
- do_y_step();
- real_move = true;
- dda_counter.y -= total_steps;
-
- if (y_direction)
- current_steps.y++;
- else
- current_steps.y--;
- }
- }
-
- if (z_can_step)
- {
- dda_counter.z += delta_steps.z;
-
- if (dda_counter.z > 0)
- {
- do_z_step();
- real_move = true;
- dda_counter.z -= total_steps;
-
- if (z_direction)
- current_steps.z++;
- else
- current_steps.z--;
- }
- }
-
- if (e_can_step)
- {
- dda_counter.e += delta_steps.e;
-
- if (dda_counter.e > 0)
- {
- do_e_step();
- real_move = true;
- dda_counter.e -= total_steps;
-
- if (e_direction)
- current_steps.e++;
- else
- current_steps.e--;
- }
- }
-
- if (f_can_step)
- {
- dda_counter.f += delta_steps.f;
-
- if (dda_counter.f > 0)
- {
- dda_counter.f -= total_steps;
- if (f_direction)
- current_steps.f++;
- else
- current_steps.f--;
- }
- }
-
-
- // wait for next step.
- // Use milli- or micro-seconds, as appropriate
- // If the only thing that changed was f keep looping
-
- if(real_move)
- {
- if(t_scale > 1)
- timestep = t_scale*current_steps.f;
- else
- timestep = current_steps.f;
- timestep = calculate_feedrate_delay((float) timestep);
- setTimer(timestep);
- }
- } while (!real_move && f_can_step);
-
- live = (x_can_step || y_can_step || z_can_step || e_can_step || f_can_step);
-
-// Wrap up at the end of a line
-
- if(!live)
- {
- disable_steppers();
- setTimer(DEFAULT_TICK);
- }
-
-}
-
-
-// Run the DDA
-
-void cartesian_dda::dda_start()
-{
- // Set up the DDA
- //sprintf(debugstring, "%d %d", x_direction, nullmove);
-
- if(nullmove)
- return;
-
- //set our direction pins as well
-#if INVERT_X_DIR == 1
- digitalWrite(X_DIR_PIN, !x_direction);
-#else
- digitalWrite(X_DIR_PIN, x_direction);
-#endif
-
-#if INVERT_Y_DIR == 1
- digitalWrite(Y_DIR_PIN, !y_direction);
-#else
- digitalWrite(Y_DIR_PIN, y_direction);
-#endif
-
-#if INVERT_Z_DIR == 1
- digitalWrite(Z_DIR_PIN, !z_direction);
-#else
- digitalWrite(Z_DIR_PIN, z_direction);
-#endif
- if(e_direction)
- ex[extruder_in_use]->set_direction(1);
- else
- ex[extruder_in_use]->set_direction(0);
-
- //turn on steppers to start moving =)
-
- enable_steppers();
-
- // extcount = 0;
-
- setTimer(DEFAULT_TICK);
- live = true;
-}
-
-
-bool cartesian_dda::can_step(byte min_pin, byte max_pin, long current, long target, byte dir)
-{
-
- //stop us if we're on target
-
- if (target == current)
- return false;
-
-#if ENDSTOPS_MIN_ENABLED == 1
-
- //stop us if we're home and still going
-
- else if(min_pin >= 0)
- {
- if (read_switch(min_pin) && !dir)
- return false;
- }
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
-
- //stop us if we're at max and still going
-
- else if(max_pin >= 0)
- {
- if (read_switch(max_pin) && dir)
- return false;
- }
-#endif
-
- // All OK - we can step
-
- return true;
-}
-
-
-void cartesian_dda::enable_steppers()
-{
-#if MOTHERBOARD > 0
- if(delta_steps.x)
- digitalWrite(X_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.y)
- digitalWrite(Y_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.z)
- digitalWrite(Z_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.e)
- ex[extruder_in_use]->enableStep();
-#endif
-}
-
-
-
-void cartesian_dda::disable_steppers()
-{
-#if MOTHERBOARD > 0
- //disable our steppers
- digitalWrite(X_ENABLE_PIN, !ENABLE_ON);
- digitalWrite(Y_ENABLE_PIN, !ENABLE_ON);
- //digitalWrite(Z_ENABLE_PIN, !ENABLE_ON);
-
- // Disabling the extrude stepper causes the backpressure to
- // turn the motor the wrong way. Leave it on.
-
- //ex[extruder_in_use]->->disableStep();
-#endif
-}
-
-/*
-
-void cartesian_dda::delayMicrosecondsInterruptible(unsigned int us)
-{
-
-#if F_CPU >= 16000000L
- // for the 16 MHz clock on most Arduino boards
-
- // for a one-microsecond delay, simply return. the overhead
- // of the function call yields a delay of approximately 1 1/8 us.
- if (--us == 0)
- return;
-
- // the following loop takes a quarter of a microsecond (4 cycles)
- // per iteration, so execute it four times for each microsecond of
- // delay requested.
- us <<= 2;
-
- // account for the time taken in the preceeding commands.
- us -= 2;
-#else
- // for the 8 MHz internal clock on the ATmega168
-
- // for a one- or two-microsecond delay, simply return. the overhead of
- // the function calls takes more than two microseconds. can't just
- // subtract two, since us is unsigned; we'd overflow.
- if (--us == 0)
- return;
- if (--us == 0)
- return;
-
- // the following loop takes half of a microsecond (4 cycles)
- // per iteration, so execute it twice for each microsecond of
- // delay requested.
- us <<= 1;
-
- // partially compensate for the time taken by the preceeding commands.
- // we can't subtract any more than this or we'd overflow w/ small delays.
- us--;
-#endif
-
- // busy wait
- __asm__ __volatile__ (
- "1: sbiw %0,1" "\n\t" // 2 cycles
- "brne 1b" : "=w" (us) : "0" (us) // 2 cycles
- );
-}
-*/
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/core.a b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/core.a
deleted file mode 100644
index 2026e003..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/core.a
+++ /dev/null
Binary files differ
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.h
deleted file mode 100644
index 2ab8a8cc..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.h
+++ /dev/null
@@ -1,230 +0,0 @@
-
-// Class for controlling each extruder
-//
-// Adrian Bowyer 14 May 2009
-
-#ifndef EXTRUDER_H
-#define EXTRUDER_H
-
-#define EXTRUDER_COUNT 2
-
-void manage_all_extruders();
-
-void new_extruder(byte e);
-
-/**********************************************************************************************
-
-* Sanguino/RepRap Motherboard v 1.0
-
-*/
-
-#if MOTHERBOARD < 2
-
-#define EXTRUDER_FORWARD true
-#define EXTRUDER_REVERSE false
-
-class extruder
-{
-private:
-
-//these our the default values for the extruder.
- byte e_speed;
- int target_celsius;
- int max_celsius;
- byte heater_low;
- byte heater_high;
- byte heater_current;
- int extrude_step_count;
-
-// These are used for temperature control
- byte count ;
- int oldT, newT;
-
-//this is for doing encoder based extruder control
-// int rpm;
-// long e_delay;
-// int error;
-// int last_extruder_error;
-// int error_delta;
- bool e_direction;
- bool valve_open;
-
-// The pins we control
- byte motor_dir_pin, motor_speed_pin, heater_pin, fan_pin, temp_pin, valve_dir_pin, valve_en_pin, step_en_pin;
-
- byte wait_till_hot();
- //byte wait_till_cool();
- void temperature_error();
- int sample_temperature();
-
-public:
-
- extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
-// void set_speed(float es);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
-// Interrupt setup and handling functions for stepper-driven extruders
-
-// void interrupt();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline void extruder::enableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, ENABLE_ON);
-}
-
-inline void extruder::disableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, !ENABLE_ON);
-}
-
-inline void extruder::step()
-{
- digitalWrite(motor_speed_pin, HIGH);
- delayMicroseconds(5);
- digitalWrite(motor_speed_pin, LOW);
-}
-
-inline void extruder::temperature_error()
-{
- Serial.print("E: ");
- Serial.println(get_temperature());
-}
-
-//warmup if we're too cold; cool down if we're too hot
-inline void extruder::wait_for_temperature()
-{
-/*
- if(wait_till_cool())
- {
- temperature_error();
- return;
- }
-*/
- if(wait_till_hot())
- temperature_error();
-}
-
-inline void extruder::set_direction(bool dir)
-{
- e_direction = dir;
- digitalWrite(motor_dir_pin, e_direction);
-}
-
-inline void extruder::set_cooler(byte sp)
-{
- if(step_en_pin >= 0) // Step enable conflicts with the fan
- return;
- analogWrite(fan_pin, sp);
-}
-
-/**********************************************************************************************
-
-* RepRap Motherboard v 1.x (x >= 1)
-* Extruder is now on RS485
-
-*/
-
-#else
-
-class extruder
-{
-private:
-
- byte address;
-
-public:
- extruder(byte a);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline extruder::extruder(byte a)
-{
- address = a;
-}
-
-inline void extruder::wait_for_temperature()
-{
-
-}
-
-inline void extruder::valve_set(bool open, int dTime)
-{
- if(open)
- talker.sendPacket(address, "V1");
- else
- talker.sendPacket(address, "V0");
- delay(dTime);
-}
-
-inline void extruder::set_direction(bool direction)
-{
-
-}
-
-inline void extruder::set_cooler(byte e_speed)
-{
-
-}
-
-inline void extruder::set_temperature(int temp)
-{
-
-}
-
-inline int extruder::get_temperature()
-{
- return 1;
-}
-
-inline void extruder::manage()
-{
-
-}
-
-inline void extruder::step()
-{
-
-}
-
-inline void extruder::enableStep()
-{
-
-}
-
-inline void extruder::disableStep()
-{
-
-}
-
-#endif
-
-extern extruder* ex[];
-extern byte extruder_in_use;
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.pde
deleted file mode 100644
index ce4593fd..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/extruder.pde
+++ /dev/null
@@ -1,308 +0,0 @@
-
-#include "parameters.h"
-#include "pins.h"
-#include "ThermistorTable.h"
-#include "extruder.h"
-
-// Keep all extruders up to temperature etc.
-
-void manage_all_extruders()
-{
- for(byte i = 0; i < EXTRUDER_COUNT; i++)
- ex[i]->manage();
-}
-
-// Select a new extruder
-
-void new_extruder(byte e)
-{
- if(e < 0)
- e = 0;
- if(e >= EXTRUDER_COUNT)
- e = EXTRUDER_COUNT - 1;
-
- if(e != extruder_in_use)
- {
- extruder_in_use = e;
- //setExtruder();
- }
-}
-
-/***************************************************************************************************************************
-
-If we have a new motherboard (V 1.x, x >= 1), the extruder is entirely controlled via the RS485, and all the functions to do
-it are simple inlines in extruder.h
-
-Otherwise, we have to do the work ourselves...
-*/
-
-#if MOTHERBOARD < 2
-extruder::extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin)
-{
- motor_dir_pin = md_pin;
- motor_speed_pin = ms_pin;
- heater_pin = h_pin;
- fan_pin = f_pin;
- temp_pin = t_pin;
- valve_dir_pin = vd_pin;
- valve_en_pin = ve_pin;
- step_en_pin = se_pin;
-
- //setup our pins
- pinMode(motor_dir_pin, OUTPUT);
- pinMode(motor_speed_pin, OUTPUT);
- pinMode(heater_pin, OUTPUT);
-
- pinMode(temp_pin, INPUT);
- pinMode(valve_dir_pin, OUTPUT);
- pinMode(valve_en_pin, OUTPUT);
-
- //initialize values
- digitalWrite(motor_dir_pin, EXTRUDER_FORWARD);
-
- analogWrite(heater_pin, 0);
- analogWrite(motor_speed_pin, 0);
- digitalWrite(valve_dir_pin, false);
- digitalWrite(valve_en_pin, 0);
-
-// The step enable pin and the fan pin are the same...
-// We can have one, or the other, but not both
-
- if(step_en_pin >= 0)
- {
- pinMode(step_en_pin, OUTPUT);
- disableStep();
- } else
- {
- pinMode(fan_pin, OUTPUT);
- analogWrite(fan_pin, 0);
- }
-
- //these our the default values for the extruder.
- e_speed = 0;
- target_celsius = 0;
- max_celsius = 0;
- heater_low = 64;
- heater_high = 255;
- heater_current = 0;
- valve_open = false;
-
-//this is for doing encoder based extruder control
-// rpm = 0;
-// e_delay = 0;
-// error = 0;
-// last_extruder_error = 0;
-// error_delta = 0;
- e_direction = EXTRUDER_FORWARD;
-
- //default to cool
- set_temperature(target_celsius);
-}
-
-
-byte extruder::wait_till_hot()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() < target_celsius - HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT > oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-
-/*
-byte extruder::wait_till_cool()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() > target_celsius + HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT < oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-*/
-
-
-
-void extruder::valve_set(bool open, int dTime)
-{
- wait_for_temperature();
- valve_open = open;
- digitalWrite(valve_dir_pin, open);
- digitalWrite(valve_en_pin, 1);
- delay(dTime);
- digitalWrite(valve_en_pin, 0);
-}
-
-
-void extruder::set_temperature(int temp)
-{
- target_celsius = temp;
- max_celsius = (temp*11)/10;
-
- // If we've turned the heat off, we might as well disable the extrude stepper
- // if(target_celsius < 1)
- // disableStep();
-}
-
-/**
-* Samples the temperature and converts it to degrees celsius.
-* Returns degrees celsius.
-*/
-int extruder::get_temperature()
-{
-#ifdef USE_THERMISTOR
- int raw = sample_temperature(temp_pin);
-
- int celsius = 0;
- byte i;
-
- for (i=1; i<NUMTEMPS; i++)
- {
- if (temptable[i][0] > raw)
- {
- celsius = temptable[i-1][1] +
- (raw - temptable[i-1][0]) *
- (temptable[i][1] - temptable[i-1][1]) /
- (temptable[i][0] - temptable[i-1][0]);
-
- break;
- }
- }
-
- // Overflow: Set to last value in the table
- if (i == NUMTEMPS) celsius = temptable[i-1][1];
- // Clamp to byte
- if (celsius > 255) celsius = 255;
- else if (celsius < 0) celsius = 0;
-
- return celsius;
-#else
- return ( 5.0 * sample_temperature() * 100.0) / 1024.0;
-#endif
-}
-
-
-
-/*
-* This function gives us an averaged sample of the analog temperature pin.
-*/
-int extruder::sample_temperature()
-{
- int raw = 0;
-
- //read in a certain number of samples
- for (byte i=0; i<TEMPERATURE_SAMPLES; i++)
- raw += analogRead(temp_pin);
-
- //average the samples
- raw = raw/TEMPERATURE_SAMPLES;
-
- //send it back.
- return raw;
-}
-
-/*!
- Manages extruder functions to keep temps, speeds etc
- at the set levels. Should be called only by manage_all_extruders(),
- which should be called in all non-trivial loops.
- o If temp is too low, don't start the motor
- o Adjust the heater power to keep the temperature at the target
- */
-void extruder::manage()
-{
- //make sure we know what our temp is.
- int current_celsius = get_temperature();
- byte newheat = 0;
-
- //put the heater into high mode if we're not at our target.
- if (current_celsius < target_celsius)
- newheat = heater_high;
- //put the heater on low if we're at our target.
- else if (current_celsius < max_celsius)
- newheat = heater_low;
-
- // Only update heat if it changed
- if (heater_current != newheat) {
- heater_current = newheat;
- analogWrite(heater_pin, heater_current);
- }
-}
-
-
-#if 0
-void extruder::set_speed(float sp)
-{
- // DC motor?
- if(step_en_pin < 0)
- {
- e_speed = (byte)sp;
- if(e_speed > 0)
- wait_for_temperature();
- analogWrite(motor_speed_pin, e_speed);
- return;
- }
-
- // No - stepper
- disableTimerInterrupt();
-
- if(sp <= 1.0e-4)
- {
- disableStep();
- e_speed = 0; // Just use this as a flag
- return;
- } else
- {
- wait_for_temperature();
- enableStep();
- e_speed = 1;
- }
-
- extrude_step_count = 0;
-
- float milliseconds_per_step = 60000.0/(E_STEPS_PER_MM*sp);
- long thousand_ticks_per_step = 4*(long)(milliseconds_per_step);
- setupTimerInterrupt();
- setTimer(thousand_ticks_per_step);
- enableTimerInterrupt();
-}
-
-void extruder::interrupt()
-{
- if(!e_speed)
- return;
- extrude_step_count++;
- if(extrude_step_count > 1000)
- {
- step();
- extrude_step_count = 0;
- }
-}
-
-#endif
-#endif
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.h
deleted file mode 100644
index fbfd83d2..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#ifndef INTERCOM_H
-#define INTERCOM_H
-
-#if MOTHERBOARD > 1
-
-#define IC_BUFFER 10
-#define MASTER_ADDRESS "00"
-
-//our RS485 pins
-#define RX_ENABLE_PIN 13
-#define TX_ENABLE_PIN 12
-
-
-class intercom
-{
- private:
- char myBuffer[IC_BUFFER];
- bool ok;
- void getPacket(char* string, int len);
-
- public:
- intercom();
- void sendPacket(byte address, char* string);
- void sendPacketWithReply(byte address, char* string, char* reply);
-
-};
-
-extern intercom talker;
-
-#endif
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.pde
deleted file mode 100644
index 5832ae5c..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/intercom.pde
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#if MOTHERBOARD > 1
-
-#include "intercom.h"
-
-intercom::intercom()
-{
- pinMode(RX_ENABLE_PIN, OUTPUT);
- pinMode(TX_ENABLE_PIN, OUTPUT);
- digitalWrite(RX_ENABLE_PIN, 0); //always listen.
- Serial1.begin(38400);
-}
-
-void intercom::sendPacket(byte address, char* string)
-{
- digitalWrite(TX_ENABLE_PIN, 1);
- Serial1.print(address, HEX);
- Serial1.print(MASTER_ADDRESS);
- Serial1.println(string);
- digitalWrite(TX_ENABLE_PIN, 0);
- getPacket(myBuffer, IC_BUFFER);
- if(myBuffer[0] != MASTER_ADDRESS[0] || myBuffer[1] != MASTER_ADDRESS[1])
- {
- // Horrible error - what to do?
- }
-}
-
-void intercom::sendPacketWithReply(byte address, char* string, char* reply)
-{
-
-}
-
-void intercom::getPacket(char* string, int len)
-{
- int i = -1;
- ok = true;
- do
- {
- while(!Serial1.available()) delay(1);
- i++;
-
- // Stop runaway buffer overflow
-
- if(i >= len)
- ok = false;
- else
- string[i] = Serial1.read();
- } while(string[i] != '\n' && ok);
- string[i] = 0;
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/parameters.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/parameters.h
deleted file mode 100644
index 1ffba5be..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/parameters.h
+++ /dev/null
@@ -1,106 +0,0 @@
-#ifndef PARAMETERS_H
-#define PARAMETERS_H
-
-// Here are the moterboard codes; set MOTHERBOARD to the right one
-// (Arduino: 0 - no longer in use)
-// Sanguino or RepRap Motherboard with direct drive extruders: 1
-// RepRap Motherboard with RS485 extruders: 2
-// Arduino Mega: 3
-
-#define MOTHERBOARD 1
-
-// Set 1s where you have endstops; 0s where you don't
-#define ENDSTOPS_MIN_ENABLED 1
-#define ENDSTOPS_MAX_ENABLED 0
-
-//our command string length
-#define COMMAND_SIZE 128
-
-// The size of the movement buffer
-
-#define BUFFER_SIZE 4
-
-// Number of microseconds between timer interrupts when no movement
-// is happening
-
-#define DEFAULT_TICK (long)1000
-
-// What delay() value to use when waiting for things to free up in milliseconds
-
-#define WAITING_DELAY 1
-
-#define INCHES_TO_MM 25.4
-
-// define the parameters of our machine.
-#define X_STEPS_PER_MM 7.99735
-#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM)
-#define X_MOTOR_STEPS 400
-#define INVERT_X_DIR 0
-
-#define Y_STEPS_PER_MM 7.99735
-#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM)
-#define Y_MOTOR_STEPS 400
-#define INVERT_Y_DIR 0
-
-#define Z_STEPS_PER_MM 320
-#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM)
-#define Z_MOTOR_STEPS 400
-#define INVERT_Z_DIR 0
-
-// For when we have a stepper-driven extruder
-// E_STEPS_PER_MM is the number of steps needed to
-// extrude 1mm out of the nozzle.
-
-#define E_STEPS_PER_MM 0.706 // 5mm diameter drive - NEMA17 pinch extruder
-//#define E_STEPS_PER_MM 2.206 // 8mm geared drive - NEMA14 pinch extruder
-#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM)
-#define E_MOTOR_STEPS 400
-
-//our maximum feedrates
-#define FAST_XY_FEEDRATE 3000.0
-#define FAST_Z_FEEDRATE 50.0
-
-// Data for acceleration calculations
-// Comment out the next line to turn accelerations off
-#define ACCELERATION_ON
-#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating
-#define SLOW_Z_FEEDRATE 20
-
-// Set to 1 if enable pins are inverting
-// For RepRap stepper boards version 1.x the enable pins are *not* inverting.
-// For RepRap stepper boards version 2.x and above the enable pins are inverting.
-#define INVERT_ENABLE_PINS 1
-
-#if INVERT_ENABLE_PINS == 1
-#define ENABLE_ON LOW
-#else
-#define ENABLE_ON HIGH
-#endif
-
-// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)
-// RepRap opto endstops are *not* inverting.
-#define ENDSTOPS_INVERTING 0
-
-// Inline interrupt control functions
-
-inline void enableTimerInterrupt()
-{
- TIMSK1 |= (1<<OCIE1A);
-}
-
-inline void disableTimerInterrupt()
-{
- TIMSK1 &= ~(1<<OCIE1A);
-}
-
-inline void setTimerCeiling(unsigned int c)
-{
- OCR1A = c;
-}
-
-inline void resetTimer()
-{
- TCNT2 = 0;
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/pins.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/pins.h
deleted file mode 100644
index dd48064a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/pins.h
+++ /dev/null
@@ -1,142 +0,0 @@
-#ifndef PINS_H
-#define PINS_H
-
-#if MOTHERBOARD == 0
-
-#error The Arduino cannot run the 5D GCode interpreter
-
-/****************************************************************************************
-* Arduino pin assignment - left here as they might be useful
-*
-****************************************************************************************/
-
-#define X_STEP_PIN (byte)2
-#define X_DIR_PIN (byte)3
-#define X_MIN_PIN (byte)4
-#define X_MAX_PIN (byte)9
-
-#define Y_STEP_PIN (byte)10
-#define Y_DIR_PIN (byte)7
-#define Y_MIN_PIN (byte)8
-#define Y_MAX_PIN (byte)13
-
-#define Z_STEP_PIN (byte)19
-#define Z_DIR_PIN (byte)18
-#define Z_MIN_PIN (byte)17
-#define Z_MAX_PIN (byte)16
-
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)11
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)12
-#define EXTRUDER_0_HEATER_PIN (byte)6
-#define EXTRUDER_0_FAN_PIN (byte)5
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)0 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)16 //NB: Conflicts with Max Z!!!!
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)15
-#define EXTRUDER_0_STEP_ENABLE_PIN 5 // 5 - NB conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* Sanguino/RepRap Motherboard with direct-drive extruders
-*
-****************************************************************************************/
-#elif MOTHERBOARD == 1
-
-#define X_STEP_PIN (byte)15
-#define X_DIR_PIN (byte)18
-#define X_MIN_PIN (byte)20
-#define X_MAX_PIN (byte)21
-#define X_ENABLE_PIN (byte)19
-
-#define Y_STEP_PIN (byte)23
-#define Y_DIR_PIN (byte)22
-#define Y_MIN_PIN (byte)25
-#define Y_MAX_PIN (byte)26
-#define Y_ENABLE_PIN (byte)19
-
-#define Z_STEP_PIN (byte)29
-#define Z_DIR_PIN (byte)30
-#define Z_MIN_PIN (byte)2
-#define Z_MAX_PIN (byte)1
-#define Z_ENABLE_PIN (byte)31
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)12
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)16
-#define EXTRUDER_0_HEATER_PIN (byte)14
-#define EXTRUDER_0_FAN_PIN (byte)3
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)4 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)17
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this
-#define EXTRUDER_0_STEP_ENABLE_PIN (byte)3 // 3 - Conflicts with the fan; set -ve if no stepper
-
-#define EXTRUDER_1_MOTOR_SPEED_PIN (byte)4
-#define EXTRUDER_1_MOTOR_DIR_PIN (byte)0
-#define EXTRUDER_1_HEATER_PIN (byte)24
-#define EXTRUDER_1_FAN_PIN (byte)7
-#define EXTRUDER_1_TEMPERATURE_PIN (byte)3 // Analogue input
-#define EXTRUDER_1_VALVE_DIR_PIN (byte) 6
-#define EXTRUDER_1_VALVE_ENABLE_PIN (byte)5 // Valve needs to be redesigned not to need this
-#define EXTRUDER_1_STEP_ENABLE_PIN (byte)-1 // 7 - Conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* RepRap Motherboard with RS485 extruders
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 2
-
-//x axis pins
-#define X_STEP_PIN 15
-#define X_DIR_PIN 18
-#define X_ENABLE_PIN 19
-#define X_MIN_PIN 20
-#define X_MAX_PIN 21
-
-//y axis pins
-#define Y_STEP_PIN 23
-#define Y_DIR_PIN 22
-#define Y_ENABLE_PIN 24
-#define Y_MIN_PIN 25
-#define Y_MAX_PIN 26
-
-//z axis pins
-#define Z_STEP_PIN 27
-#define Z_DIR_PIN 28
-#define Z_ENABLE_PIN 29
-#define Z_MIN_PIN 30
-#define Z_MAX_PIN 31
-
-//our pin for debugging.
-#define DEBUG_PIN 0
-
-//our SD card pins
-#define SD_CARD_WRITE 2
-#define SD_CARD_DETECT 3
-#define SD_CARD_SELECT 4
-
-//our RS485 pins
-#define TX_ENABLE_PIN 12
-#define RX_ENABLE_PIN 13
-
-//pin for controlling the PSU.
-#define PS_ON_PIN 14
-
-/****************************************************************************************
-* Arduino Mega pin assignment
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 3
-
-#error The Arduino Mega is not yet implemented for the 5D GCode interpreter
-
-#else
-
-#error Unknown MOTHERBOARD value in parameters.h
-
-#endif
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/process_g_code.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/process_g_code.pde
deleted file mode 100644
index 941c180a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/process_g_code.pde
+++ /dev/null
@@ -1,525 +0,0 @@
-
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-/* bit-flags for commands and parameters */
-#define GCODE_G (1<<0)
-#define GCODE_M (1<<1)
-#define GCODE_P (1<<2)
-#define GCODE_X (1<<3)
-#define GCODE_Y (1<<4)
-#define GCODE_Z (1<<5)
-#define GCODE_I (1<<6)
-#define GCODE_J (1<<7)
-#define GCODE_K (1<<8)
-#define GCODE_F (1<<9)
-#define GCODE_S (1<<10)
-#define GCODE_Q (1<<11)
-#define GCODE_R (1<<12)
-#define GCODE_E (1<<13)
-#define GCODE_T (1<<14)
-
-
-#define PARSE_INT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_int(str, &val, &seen, flag); \
- break;
-
-#define PARSE_FLOAT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_float(str, &val, &seen, flag); \
- break;
-
-/* gcode line parse results */
-struct GcodeParser
-{
- unsigned int seen;
- int G;
- int M;
- int T;
- float P;
- float X;
- float Y;
- float Z;
- float E;
- float I;
- float J;
- float F;
- float S;
- float R;
- float Q;
-};
-
-
-//our command string
-char cmdbuffer[COMMAND_SIZE];
-char c = '?';
-byte serial_count = 0;
-boolean comment = false;
-FloatPoint fp;
-FloatPoint sp;
-
-// The following three inline functions are used for things like return to 0
-
-inline void specialMoveX(const float& x, const float& feed)
-{
- sp = where_i_am;
- sp.x = x;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveY(const float& y, const float& feed)
-{
- sp = where_i_am;
- sp.y = y;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveZ(const float& z, const float& feed)
-{
- sp = where_i_am;
- sp.z = z;
- sp.f = feed;
- qMove(sp);
-}
-
-//our feedrate variables.
-//float feedrate = SLOW_XY_FEEDRATE;
-
-/* keep track of the last G code - this is the command mode to use
- * if there is no command in the current string
- */
-int last_gcode_g = -1;
-
-boolean abs_mode = true; //0 = incremental; 1 = absolute
-
-float extruder_speed = 0;
-
-int scan_int(char *str, int *valp);
-int scan_float(char *str, float *valp);
-
-GcodeParser gc; /* string parse result */
-
-
-//init our string processing
-inline void init_process_string()
-{
- serial_count = 0;
- comment = false;
-}
-
-// Get a command and process it
-
-void get_and_do_command()
-{
- //read in characters if we got them.
- if (Serial.available())
- {
- c = Serial.read();
- if(c == '\r')
- c = '\n';
- // Throw away control chars except \n
- if(c >= ' ' || c == '\n')
- {
-
- //newlines are ends of commands.
- if (c != '\n')
- {
- // Start of comment - ignore any bytes received from now on
- if (c == ';')
- comment = true;
-
- // If we're not in comment mode, add it to our array.
- if (!comment)
- cmdbuffer[serial_count++] = c;
- }
-
- }
- }
-
- // Data runaway?
- if(serial_count >= COMMAND_SIZE)
- init_process_string();
-
- //if we've got a real command, do it
- if (serial_count && c == '\n')
- {
- // Terminate string
- cmdbuffer[serial_count] = 0;
-
- //process our command!
- process_string(cmdbuffer, serial_count);
-
- //clear command.
- init_process_string();
-
- // Say we're ready for the next one
-
- if(debugstring[0] != 0)
- {
- Serial.print("ok ");
- Serial.println(debugstring);
- debugstring[0] = 0;
- } else
- Serial.println("ok");
- }
-}
-
-
-
-int parse_string(struct GcodeParser * gc, char instruction[ ], int size)
-{
- int ind;
- int len; /* length of parameter argument */
-
- gc->seen = 0;
-
- len=0;
- /* scan the string for commands and parameters, recording the arguments for each,
- * and setting the seen flag for each that is seen
- */
- for (ind=0; ind<size; ind += (1+len))
- {
- len = 0;
- switch (instruction[ind])
- {
- PARSE_INT('G', &instruction[ind+1], len, gc->G, gc->seen, GCODE_G);
- PARSE_INT('M', &instruction[ind+1], len, gc->M, gc->seen, GCODE_M);
- PARSE_INT('T', &instruction[ind+1], len, gc->T, gc->seen, GCODE_T);
- PARSE_FLOAT('S', &instruction[ind+1], len, gc->S, gc->seen, GCODE_S);
- PARSE_FLOAT('P', &instruction[ind+1], len, gc->P, gc->seen, GCODE_P);
- PARSE_FLOAT('X', &instruction[ind+1], len, gc->X, gc->seen, GCODE_X);
- PARSE_FLOAT('Y', &instruction[ind+1], len, gc->Y, gc->seen, GCODE_Y);
- PARSE_FLOAT('Z', &instruction[ind+1], len, gc->Z, gc->seen, GCODE_Z);
- PARSE_FLOAT('I', &instruction[ind+1], len, gc->I, gc->seen, GCODE_I);
- PARSE_FLOAT('J', &instruction[ind+1], len, gc->J, gc->seen, GCODE_J);
- PARSE_FLOAT('F', &instruction[ind+1], len, gc->F, gc->seen, GCODE_F);
- PARSE_FLOAT('R', &instruction[ind+1], len, gc->R, gc->seen, GCODE_R);
- PARSE_FLOAT('Q', &instruction[ind+1], len, gc->Q, gc->seen, GCODE_Q);
- PARSE_FLOAT('E', &instruction[ind+1], len, gc->E, gc->seen, GCODE_E);
- default:
- break;
- }
- }
-}
-
-
-//Read the string and execute instructions
-void process_string(char instruction[], int size)
-{
- //the character / means delete block... used for comments and stuff.
- if (instruction[0] == '/')
- return;
-
- float fr;
-
- fp.x = 0.0;
- fp.y = 0.0;
- fp.z = 0.0;
- fp.e = 0.0;
- fp.f = 0.0;
-
- //get all our parameters!
- parse_string(&gc, instruction, size);
- /* if no command was seen, but parameters were, then use the last G code as
- * the current command
- */
- if ((!(gc.seen & (GCODE_G | GCODE_M | GCODE_T))) &&
- ((gc.seen != 0) &&
- (last_gcode_g >= 0))
- )
- {
- /* yes - so use the previous command with the new parameters */
- gc.G = last_gcode_g;
- gc.seen |= GCODE_G;
- }
- //did we get a gcode?
- if (gc.seen & GCODE_G)
- {
- last_gcode_g = gc.G; /* remember this for future instructions */
- fp = where_i_am;
- if (abs_mode)
- {
- if (gc.seen & GCODE_X)
- fp.x = gc.X;
- if (gc.seen & GCODE_Y)
- fp.y = gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z = gc.Z;
- if (gc.seen & GCODE_E)
- fp.e = gc.E;
- }
- else
- {
- if (gc.seen & GCODE_X)
- fp.x += gc.X;
- if (gc.seen & GCODE_Y)
- fp.y += gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z += gc.Z;
- if (gc.seen & GCODE_E)
- fp.e += gc.E;
- }
-
- // Get feedrate if supplied - feedrates are always absolute???
- if ( gc.seen & GCODE_F )
- fp.f = gc.F;
-
- // Process the buffered move commands first
- // If we get one, return immediately
-
- switch (gc.G)
- {
- //Rapid move
- case 0:
- fr = fp.f;
- fp.f = FAST_XY_FEEDRATE;
- qMove(fp);
- fp.f = fr;
- return;
-
- // Controlled move
- case 1:
- qMove(fp);
- return;
-
- //go home.
- case 28:
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x - 5, FAST_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 250, FAST_XY_FEEDRATE);
- where_i_am.x = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x + 1, SLOW_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 10, SLOW_XY_FEEDRATE);
- where_i_am.x = 0;
-
- specialMoveY(where_i_am.y - 5, FAST_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 250, FAST_XY_FEEDRATE);
- where_i_am.y = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveY(where_i_am.y + 1, SLOW_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 10, SLOW_XY_FEEDRATE);
- where_i_am.y = 0;
-
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z - 0.5, FAST_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 250, FAST_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z + 1, SLOW_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 2, SLOW_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_XY_FEEDRATE; // Most sensible feedrate to leave it in
-
- return;
-
-
- default:
- break;
- }
-
- // Non-buffered G commands
- // Wait till the buffer q is empty first
-
- while(!qEmpty()) delay(WAITING_DELAY);
- switch (gc.G)
- {
-
- //Dwell
- case 4:
- delay((int)(gc.P + 0.5));
- break;
-
- //Inches for Units
- case 20:
- setUnits(false);
- break;
-
- //mm for Units
- case 21:
- setUnits(true);
- break;
-
- //Absolute Positioning
- case 90:
- abs_mode = true;
- break;
-
- //Incremental Positioning
- case 91:
- abs_mode = false;
- break;
-
- //Set position as fp
- case 92:
- setPosition(fp);
- break;
-
- default:
- Serial.print("huh? G");
- Serial.println(gc.G, DEC);
- }
- }
-
-
-
-
- //find us an m code.
- if (gc.seen & GCODE_M)
- {
- // Wait till the q is empty first
- while(!qEmpty()) delay(WAITING_DELAY);
-
- switch (gc.M)
- {
- //TODO: this is a bug because search_string returns 0. gotta fix that.
- case 0:
- break;
- /*
- case 0:
- //todo: stop program
- break;
-
- case 1:
- //todo: optional stop
- break;
-
- case 2:
- //todo: program end
- break;
- */
-
-// Now, with E codes, there is no longer any idea of turning the extruder on or off.
-// (But see valve on/off below.)
-
-/*
- //turn extruder on, forward
- case 101:
- ex[extruder_in_use]->set_direction(1);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder on, reverse
- case 102:
- ex[extruder_in_use]->set_direction(0);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder off
- case 103:
- ex[extruder_in_use]->set_speed(0);
- break;
-*/
- //custom code for temperature control
- case 104:
- if (gc.seen & GCODE_S)
- {
- ex[extruder_in_use]->set_temperature((int)gc.S);
- }
- break;
-
- //custom code for temperature reading
- case 105:
- Serial.print("T:");
- Serial.println(ex[extruder_in_use]->get_temperature());
- break;
-
- //turn fan on
- case 106:
- ex[extruder_in_use]->set_cooler(255);
- break;
-
- //turn fan off
- case 107:
- ex[extruder_in_use]->set_cooler(0);
- break;
-/*
-// Extruder speed is now entirely controlled by E codes
- //set max extruder speed, 0-255 PWM
- case 108:
- if (gc.seen & GCODE_S)
- extruder_speed = gc.S;
- break;
-*/
-
-// The valve (real, or virtual...) is now the way to control any extruder (such as
-// a pressurised paste extruder) that cannot move using E codes.
-
- // Open the valve
- case 126:
- ex[extruder_in_use]->valve_set(true, (int)(gc.P + 0.5));
- break;
-
- // Close the valve
- case 127:
- ex[extruder_in_use]->valve_set(false, (int)(gc.P + 0.5));
- break;
-
-
- default:
- Serial.print("Huh? M");
- Serial.println(gc.M, DEC);
- }
-
-
-
- }
-
-// Tool (i.e. extruder) change?
-
- if (gc.seen & GCODE_T)
- {
- while(!qEmpty()) delay(WAITING_DELAY);
- new_extruder(gc.T);
- }
-}
-
-int scan_float(char *str, float *valp, unsigned int *seen, unsigned int flag)
-{
- float res;
- int len;
- char *end;
-
- res = (float)strtod(str, &end);
-
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-int scan_int(char *str, int *valp, unsigned int *seen, unsigned int flag)
-{
- int res;
- int len;
- char *end;
-
- res = (int)strtol(str, &end, 10);
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/vectors.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/vectors.h
deleted file mode 100644
index 938cfe0c..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/applet/vectors.h
+++ /dev/null
@@ -1,130 +0,0 @@
-// Structs to hold points in 5D space.
-
-
-#ifndef VECTORS_H
-#define VECTORS_H
-
-#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
-
-// Real-world units
-struct FloatPoint
-{
- float x; // Coordinate axes
- float y;
- float z;
- float e; // Extrude length
- float f; // Feedrate
-};
-
-inline FloatPoint operator+(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline FloatPoint operator-(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-// NB - the next gives neither the scalar nor the vector product
-
-inline FloatPoint operator*(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x * b.x;
- result.y = a.y * b.y;
- result.z = a.z * b.z;
- result.e = a.e * b.e;
- result.f = a.f * b.f;
- return result;
-}
-
-// Can't use fabs for this as it's defined somewhere in a #define
-
-inline FloatPoint fabsv(const FloatPoint& a)
-{
- FloatPoint result;
- result.x = fabs(a.x);
- result.y = fabs(a.y);
- result.z = fabs(a.z);
- result.e = fabs(a.e);
- result.f = fabs(a.f);
- return result;
-}
-
-
-// Integer numbers of steps
-struct LongPoint
-{
- long x; // Coordinates
- long y;
- long z;
- long e; // Extrusion
- long f; // Feedrate
-};
-
-inline LongPoint operator+(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline LongPoint operator-(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-inline LongPoint absv(const LongPoint& a)
-{
- LongPoint result;
- result.x = abs(a.x);
- result.y = abs(a.y);
- result.z = abs(a.z);
- result.e = abs(a.e);
- result.f = abs(a.f);
- return result;
-}
-
-
-inline LongPoint roundv(const FloatPoint& a)
-{
- LongPoint result;
- result.x = round(a.x);
- result.y = round(a.y);
- result.z = round(a.z);
- result.e = round(a.e);
- result.f = round(a.f);
- return result;
-}
-
-inline LongPoint to_steps(const FloatPoint& units, const FloatPoint& position)
-{
- return roundv(units*position);
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h
deleted file mode 100644
index bf6e9217..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * This class controls the movement of the RepRap machine.
- * It implements a DDA in four dimensions, so the length of extruded
- * filament is treated as a variable, just like X, Y, and Z.
- *
- * Adrian Bowyer 9 May 2009
- */
-
-#ifndef CARTESIAN_DDA_H
-#define CARTESIAN_DDA_H
-
-// Main class for moving the RepRap machine about
-
-class cartesian_dda
-{
-private:
-
- //extruder* ext; // The extruder I'm currently using - keep this up to date...
-
- FloatPoint units; // Factors for converting either mm or inches to steps
-
- FloatPoint target_position; // Where it's going
- FloatPoint delta_position; // The difference between the two
- float distance; // How long the path is
-
- LongPoint current_steps; // Similar information as above in steps rather than units
- LongPoint target_steps;
- LongPoint delta_steps;
- LongPoint dda_counter; // DDA error-accumulation variables
- long t_scale; // When doing lots of t steps, scale them so the DDA doesn't spend for ever on them
-
- byte x_direction; // Am I going in the + or - direction?
- byte y_direction;
- byte z_direction;
- byte e_direction;
- byte f_direction;
-
- bool x_can_step; // Am I not at an endstop? Have I not reached the target? etc.
- bool y_can_step;
- bool z_can_step;
- bool e_can_step;
- bool f_can_step;
-
-// Variables for acceleration calculations
-
- long total_steps; // The number of steps to take along the longest movement axis
-
- long timestep; // microseconds
- bool nullmove; // this move is zero length
- bool real_move; // Flag to know if we've changed something physical
- volatile bool live; // Flag for when we're plotting a line
-
-// Internal functions that need not concern the user
-
- // Take a single step
-
- void do_x_step();
- void do_y_step();
- void do_z_step();
- void do_e_step();
-
- // Can this axis step?
-
- bool can_step(byte min_pin, byte max_pin, long current, long target, byte dir);
-
- // Read a limit switch
-
- bool read_switch(byte pin);
-
- // Work out the number of microseconds between steps
-
- long calculate_feedrate_delay(const float& feedrate);
-
- // Switch the steppers on and off
-
- void enable_steppers();
- void disable_steppers();
-
- // Custom short delay function (microseconds)
-
- //void delayMicrosecondsInterruptible(unsigned int us);
-
-
-public:
-
- cartesian_dda();
-
- // Set where I'm going
-
- void set_target(const FloatPoint& p);
-
- // Start the DDA
-
- void dda_start();
-
- // Do one step of the DDA
-
- void dda_step();
-
- // Are we running at the moment?
-
- bool active();
-
- // True for mm; false for inches
-
- void set_units(bool using_mm);
-
- // Record the selection of a new extruder
-
- //void set_extruder(extruder* ex);
-};
-
-// Short functions inline to save memory; particularly useful in the Arduino
-
-
-//inline void cartesian_dda::set_extruder(extruder* ex)
-//{
-// ext = ex;
-//}
-
-inline bool cartesian_dda::active()
-{
- return live;
-}
-
-inline void cartesian_dda::do_x_step()
-{
- digitalWrite(X_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(X_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_y_step()
-{
- digitalWrite(Y_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Y_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_z_step()
-{
- digitalWrite(Z_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Z_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_e_step()
-{
- ex[extruder_in_use]->step();
-}
-
-inline long cartesian_dda::calculate_feedrate_delay(const float& feedrate)
-{
-
- // Calculate delay between steps in microseconds. Here it is in English:
- // (feedrate is in mm/minute, distance is in mm)
- // 60000000.0*distance/feedrate = move duration in microseconds
- // move duration/total_steps = time between steps for master axis.
-
- return round( (distance*60000000.0) / (feedrate*(float)total_steps) );
-}
-
-inline bool cartesian_dda::read_switch(byte pin)
-{
- //dual read as crude debounce
-
- #if ENDSTOPS_INVERTING == 1
- return !digitalRead(pin) && !digitalRead(pin);
- #else
- return digitalRead(pin) && digitalRead(pin);
- #endif
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h.dist b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h.dist
deleted file mode 100644
index bf6e9217..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.h.dist
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * This class controls the movement of the RepRap machine.
- * It implements a DDA in four dimensions, so the length of extruded
- * filament is treated as a variable, just like X, Y, and Z.
- *
- * Adrian Bowyer 9 May 2009
- */
-
-#ifndef CARTESIAN_DDA_H
-#define CARTESIAN_DDA_H
-
-// Main class for moving the RepRap machine about
-
-class cartesian_dda
-{
-private:
-
- //extruder* ext; // The extruder I'm currently using - keep this up to date...
-
- FloatPoint units; // Factors for converting either mm or inches to steps
-
- FloatPoint target_position; // Where it's going
- FloatPoint delta_position; // The difference between the two
- float distance; // How long the path is
-
- LongPoint current_steps; // Similar information as above in steps rather than units
- LongPoint target_steps;
- LongPoint delta_steps;
- LongPoint dda_counter; // DDA error-accumulation variables
- long t_scale; // When doing lots of t steps, scale them so the DDA doesn't spend for ever on them
-
- byte x_direction; // Am I going in the + or - direction?
- byte y_direction;
- byte z_direction;
- byte e_direction;
- byte f_direction;
-
- bool x_can_step; // Am I not at an endstop? Have I not reached the target? etc.
- bool y_can_step;
- bool z_can_step;
- bool e_can_step;
- bool f_can_step;
-
-// Variables for acceleration calculations
-
- long total_steps; // The number of steps to take along the longest movement axis
-
- long timestep; // microseconds
- bool nullmove; // this move is zero length
- bool real_move; // Flag to know if we've changed something physical
- volatile bool live; // Flag for when we're plotting a line
-
-// Internal functions that need not concern the user
-
- // Take a single step
-
- void do_x_step();
- void do_y_step();
- void do_z_step();
- void do_e_step();
-
- // Can this axis step?
-
- bool can_step(byte min_pin, byte max_pin, long current, long target, byte dir);
-
- // Read a limit switch
-
- bool read_switch(byte pin);
-
- // Work out the number of microseconds between steps
-
- long calculate_feedrate_delay(const float& feedrate);
-
- // Switch the steppers on and off
-
- void enable_steppers();
- void disable_steppers();
-
- // Custom short delay function (microseconds)
-
- //void delayMicrosecondsInterruptible(unsigned int us);
-
-
-public:
-
- cartesian_dda();
-
- // Set where I'm going
-
- void set_target(const FloatPoint& p);
-
- // Start the DDA
-
- void dda_start();
-
- // Do one step of the DDA
-
- void dda_step();
-
- // Are we running at the moment?
-
- bool active();
-
- // True for mm; false for inches
-
- void set_units(bool using_mm);
-
- // Record the selection of a new extruder
-
- //void set_extruder(extruder* ex);
-};
-
-// Short functions inline to save memory; particularly useful in the Arduino
-
-
-//inline void cartesian_dda::set_extruder(extruder* ex)
-//{
-// ext = ex;
-//}
-
-inline bool cartesian_dda::active()
-{
- return live;
-}
-
-inline void cartesian_dda::do_x_step()
-{
- digitalWrite(X_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(X_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_y_step()
-{
- digitalWrite(Y_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Y_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_z_step()
-{
- digitalWrite(Z_STEP_PIN, HIGH);
- delayMicroseconds(5);
- digitalWrite(Z_STEP_PIN, LOW);
-}
-
-inline void cartesian_dda::do_e_step()
-{
- ex[extruder_in_use]->step();
-}
-
-inline long cartesian_dda::calculate_feedrate_delay(const float& feedrate)
-{
-
- // Calculate delay between steps in microseconds. Here it is in English:
- // (feedrate is in mm/minute, distance is in mm)
- // 60000000.0*distance/feedrate = move duration in microseconds
- // move duration/total_steps = time between steps for master axis.
-
- return round( (distance*60000000.0) / (feedrate*(float)total_steps) );
-}
-
-inline bool cartesian_dda::read_switch(byte pin)
-{
- //dual read as crude debounce
-
- #if ENDSTOPS_INVERTING == 1
- return !digitalRead(pin) && !digitalRead(pin);
- #else
- return digitalRead(pin) && digitalRead(pin);
- #endif
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.pde
deleted file mode 100644
index 09698334..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/cartesian_dda.pde
+++ /dev/null
@@ -1,468 +0,0 @@
-#include <stdio.h>
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-
-// Initialise X, Y and Z. The extruder is initialized
-// separately.
-
-cartesian_dda::cartesian_dda()
-{
- live = false;
- nullmove = false;
-
- // Default is going forward
-
- x_direction = 1;
- y_direction = 1;
- z_direction = 1;
- e_direction = 1;
- f_direction = 1;
-
- // Default to the origin and not going anywhere
-
- target_position.x = 0.0;
- target_position.y = 0.0;
- target_position.z = 0.0;
- target_position.e = 0.0;
- target_position.f = SLOW_XY_FEEDRATE;
-
- // Set up the pin directions
-
- pinMode(X_STEP_PIN, OUTPUT);
- pinMode(X_DIR_PIN, OUTPUT);
-
- pinMode(Y_STEP_PIN, OUTPUT);
- pinMode(Y_DIR_PIN, OUTPUT);
-
- pinMode(Z_STEP_PIN, OUTPUT);
- pinMode(Z_DIR_PIN, OUTPUT);
-
-#if MOTHERBOARD > 0
- pinMode(X_ENABLE_PIN, OUTPUT);
- pinMode(Y_ENABLE_PIN, OUTPUT);
- pinMode(Z_ENABLE_PIN, OUTPUT);
-#endif
-
- //turn the motors off at the start.
-
- disable_steppers();
-
-#if ENDSTOPS_MIN_ENABLED == 1
- pinMode(X_MIN_PIN, INPUT);
- pinMode(Y_MIN_PIN, INPUT);
- pinMode(Z_MIN_PIN, INPUT);
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
- pinMode(X_MAX_PIN, INPUT);
- pinMode(Y_MAX_PIN, INPUT);
- pinMode(Z_MAX_PIN, INPUT);
-#endif
-
- // Default units are mm
-
- set_units(true);
-}
-
-// Switch between mm and inches
-
-void cartesian_dda::set_units(bool using_mm)
-{
- if(using_mm)
- {
- units.x = X_STEPS_PER_MM;
- units.y = Y_STEPS_PER_MM;
- units.z = Z_STEPS_PER_MM;
- units.e = E_STEPS_PER_MM;
- units.f = 1.0;
- } else
- {
- units.x = X_STEPS_PER_INCH;
- units.y = Y_STEPS_PER_INCH;
- units.z = Z_STEPS_PER_INCH;
- units.e = E_STEPS_PER_INCH;
- units.f = 1.0;
- }
-}
-
-
-void cartesian_dda::set_target(const FloatPoint& p)
-{
- target_position = p;
- nullmove = false;
-
- //figure our deltas.
-
- delta_position = fabsv(target_position - where_i_am);
-
- // The feedrate values refer to distance in (X, Y, Z) space, so ignore e and f
- // values unless they're the only thing there.
-
- FloatPoint squares = delta_position*delta_position;
- distance = squares.x + squares.y + squares.z;
- // If we are 0, only thing changing is e
- if(distance <= 0.0)
- distance = squares.e;
- // If we are still 0, only thing changing is f
- if(distance <= 0.0)
- distance = squares.f;
- distance = sqrt(distance);
-
- //set our steps current, target, and delta
-
- current_steps = to_steps(units, where_i_am);
- target_steps = to_steps(units, target_position);
- delta_steps = absv(target_steps - current_steps);
-
- // find the dominant axis.
- // NB we ignore the f values here, as it takes no time to take a step in time :-)
-
- total_steps = max(delta_steps.x, delta_steps.y);
- total_steps = max(total_steps, delta_steps.z);
- total_steps = max(total_steps, delta_steps.e);
-
- // If we're not going anywhere, flag the fact
-
- if(total_steps == 0)
- {
- nullmove = true;
- where_i_am = p;
- return;
- }
-
-#ifndef ACCELERATION_ON
- current_steps.f = round(target_position.f);
-#endif
-
- delta_steps.f = abs(target_steps.f - current_steps.f);
-
- // Rescale the feedrate so it doesn't take lots of steps to do
-
- t_scale = 1;
- if(delta_steps.f > total_steps)
- {
- t_scale = delta_steps.f/total_steps;
- if(t_scale >= 3)
- {
- target_steps.f = target_steps.f/t_scale;
- current_steps.f = current_steps.f/t_scale;
- delta_steps.f = abs(target_steps.f - current_steps.f);
- if(delta_steps.f > total_steps)
- total_steps = delta_steps.f;
- } else
- {
- t_scale = 1;
- total_steps = delta_steps.f;
- }
- }
-
- //what is our direction?
-
- x_direction = (target_position.x >= where_i_am.x);
- y_direction = (target_position.y >= where_i_am.y);
- z_direction = (target_position.z >= where_i_am.z);
- e_direction = (target_position.e >= where_i_am.e);
- f_direction = (target_position.f >= where_i_am.f);
-
- dda_counter.x = -total_steps/2;
- dda_counter.y = dda_counter.x;
- dda_counter.z = dda_counter.x;
- dda_counter.e = dda_counter.x;
- dda_counter.f = dda_counter.x;
-
- where_i_am = p;
-
- return;
-}
-
-
-
-void cartesian_dda::dda_step()
-{
- if(!live)
- return;
-
- do
- {
- x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
- y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
- z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
- e_can_step = can_step(-1, -1, current_steps.e, target_steps.e, e_direction);
- f_can_step = can_step(-1, -1, current_steps.f, target_steps.f, f_direction);
-
- real_move = false;
-
- if (x_can_step)
- {
- dda_counter.x += delta_steps.x;
-
- if (dda_counter.x > 0)
- {
- do_x_step();
- real_move = true;
- dda_counter.x -= total_steps;
-
- if (x_direction)
- current_steps.x++;
- else
- current_steps.x--;
- }
- }
-
- if (y_can_step)
- {
- dda_counter.y += delta_steps.y;
-
- if (dda_counter.y > 0)
- {
- do_y_step();
- real_move = true;
- dda_counter.y -= total_steps;
-
- if (y_direction)
- current_steps.y++;
- else
- current_steps.y--;
- }
- }
-
- if (z_can_step)
- {
- dda_counter.z += delta_steps.z;
-
- if (dda_counter.z > 0)
- {
- do_z_step();
- real_move = true;
- dda_counter.z -= total_steps;
-
- if (z_direction)
- current_steps.z++;
- else
- current_steps.z--;
- }
- }
-
- if (e_can_step)
- {
- dda_counter.e += delta_steps.e;
-
- if (dda_counter.e > 0)
- {
- do_e_step();
- real_move = true;
- dda_counter.e -= total_steps;
-
- if (e_direction)
- current_steps.e++;
- else
- current_steps.e--;
- }
- }
-
- if (f_can_step)
- {
- dda_counter.f += delta_steps.f;
-
- if (dda_counter.f > 0)
- {
- dda_counter.f -= total_steps;
- if (f_direction)
- current_steps.f++;
- else
- current_steps.f--;
- }
- }
-
-
- // wait for next step.
- // Use milli- or micro-seconds, as appropriate
- // If the only thing that changed was f keep looping
-
- if(real_move)
- {
- if(t_scale > 1)
- timestep = t_scale*current_steps.f;
- else
- timestep = current_steps.f;
- timestep = calculate_feedrate_delay((float) timestep);
- setTimer(timestep);
- }
- } while (!real_move && f_can_step);
-
- live = (x_can_step || y_can_step || z_can_step || e_can_step || f_can_step);
-
-// Wrap up at the end of a line
-
- if(!live)
- {
- disable_steppers();
- setTimer(DEFAULT_TICK);
- }
-
-}
-
-
-// Run the DDA
-
-void cartesian_dda::dda_start()
-{
- // Set up the DDA
- //sprintf(debugstring, "%d %d", x_direction, nullmove);
-
- if(nullmove)
- return;
-
- //set our direction pins as well
-#if INVERT_X_DIR == 1
- digitalWrite(X_DIR_PIN, !x_direction);
-#else
- digitalWrite(X_DIR_PIN, x_direction);
-#endif
-
-#if INVERT_Y_DIR == 1
- digitalWrite(Y_DIR_PIN, !y_direction);
-#else
- digitalWrite(Y_DIR_PIN, y_direction);
-#endif
-
-#if INVERT_Z_DIR == 1
- digitalWrite(Z_DIR_PIN, !z_direction);
-#else
- digitalWrite(Z_DIR_PIN, z_direction);
-#endif
- if(e_direction)
- ex[extruder_in_use]->set_direction(1);
- else
- ex[extruder_in_use]->set_direction(0);
-
- //turn on steppers to start moving =)
-
- enable_steppers();
-
- // extcount = 0;
-
- setTimer(DEFAULT_TICK);
- live = true;
-}
-
-
-bool cartesian_dda::can_step(byte min_pin, byte max_pin, long current, long target, byte dir)
-{
-
- //stop us if we're on target
-
- if (target == current)
- return false;
-
-#if ENDSTOPS_MIN_ENABLED == 1
-
- //stop us if we're home and still going
-
- else if(min_pin >= 0)
- {
- if (read_switch(min_pin) && !dir)
- return false;
- }
-#endif
-
-#if ENDSTOPS_MAX_ENABLED == 1
-
- //stop us if we're at max and still going
-
- else if(max_pin >= 0)
- {
- if (read_switch(max_pin) && dir)
- return false;
- }
-#endif
-
- // All OK - we can step
-
- return true;
-}
-
-
-void cartesian_dda::enable_steppers()
-{
-#if MOTHERBOARD > 0
- if(delta_steps.x)
- digitalWrite(X_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.y)
- digitalWrite(Y_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.z)
- digitalWrite(Z_ENABLE_PIN, ENABLE_ON);
- if(delta_steps.e)
- ex[extruder_in_use]->enableStep();
-#endif
-}
-
-
-
-void cartesian_dda::disable_steppers()
-{
-#if MOTHERBOARD > 0
- //disable our steppers
- digitalWrite(X_ENABLE_PIN, !ENABLE_ON);
- digitalWrite(Y_ENABLE_PIN, !ENABLE_ON);
- digitalWrite(Z_ENABLE_PIN, !ENABLE_ON);
-
- // Disabling the extrude stepper causes the backpressure to
- // turn the motor the wrong way. Leave it on.
-
- //ex[extruder_in_use]->->disableStep();
-#endif
-}
-
-/*
-
-void cartesian_dda::delayMicrosecondsInterruptible(unsigned int us)
-{
-
-#if F_CPU >= 16000000L
- // for the 16 MHz clock on most Arduino boards
-
- // for a one-microsecond delay, simply return. the overhead
- // of the function call yields a delay of approximately 1 1/8 us.
- if (--us == 0)
- return;
-
- // the following loop takes a quarter of a microsecond (4 cycles)
- // per iteration, so execute it four times for each microsecond of
- // delay requested.
- us <<= 2;
-
- // account for the time taken in the preceeding commands.
- us -= 2;
-#else
- // for the 8 MHz internal clock on the ATmega168
-
- // for a one- or two-microsecond delay, simply return. the overhead of
- // the function calls takes more than two microseconds. can't just
- // subtract two, since us is unsigned; we'd overflow.
- if (--us == 0)
- return;
- if (--us == 0)
- return;
-
- // the following loop takes half of a microsecond (4 cycles)
- // per iteration, so execute it twice for each microsecond of
- // delay requested.
- us <<= 1;
-
- // partially compensate for the time taken by the preceeding commands.
- // we can't subtract any more than this or we'd overflow w/ small delays.
- us--;
-#endif
-
- // busy wait
- __asm__ __volatile__ (
- "1: sbiw %0,1" "\n\t" // 2 cycles
- "brne 1b" : "=w" (us) : "0" (us) // 2 cycles
- );
-}
-*/
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/download-copy b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/download-copy
deleted file mode 100755
index c3a47867..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/download-copy
+++ /dev/null
@@ -1,7 +0,0 @@
-#!/bin/sh
-cp cartesian_dda.h.dist cartesian_dda.h
-cp extruder.h.dist extruder.h
-cp parameters.h.dist parameters.h
-cp pins.h.dist pins.h
-cp vectors.h.dist vectors.h
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h
deleted file mode 100644
index 2ab8a8cc..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h
+++ /dev/null
@@ -1,230 +0,0 @@
-
-// Class for controlling each extruder
-//
-// Adrian Bowyer 14 May 2009
-
-#ifndef EXTRUDER_H
-#define EXTRUDER_H
-
-#define EXTRUDER_COUNT 2
-
-void manage_all_extruders();
-
-void new_extruder(byte e);
-
-/**********************************************************************************************
-
-* Sanguino/RepRap Motherboard v 1.0
-
-*/
-
-#if MOTHERBOARD < 2
-
-#define EXTRUDER_FORWARD true
-#define EXTRUDER_REVERSE false
-
-class extruder
-{
-private:
-
-//these our the default values for the extruder.
- byte e_speed;
- int target_celsius;
- int max_celsius;
- byte heater_low;
- byte heater_high;
- byte heater_current;
- int extrude_step_count;
-
-// These are used for temperature control
- byte count ;
- int oldT, newT;
-
-//this is for doing encoder based extruder control
-// int rpm;
-// long e_delay;
-// int error;
-// int last_extruder_error;
-// int error_delta;
- bool e_direction;
- bool valve_open;
-
-// The pins we control
- byte motor_dir_pin, motor_speed_pin, heater_pin, fan_pin, temp_pin, valve_dir_pin, valve_en_pin, step_en_pin;
-
- byte wait_till_hot();
- //byte wait_till_cool();
- void temperature_error();
- int sample_temperature();
-
-public:
-
- extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
-// void set_speed(float es);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
-// Interrupt setup and handling functions for stepper-driven extruders
-
-// void interrupt();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline void extruder::enableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, ENABLE_ON);
-}
-
-inline void extruder::disableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, !ENABLE_ON);
-}
-
-inline void extruder::step()
-{
- digitalWrite(motor_speed_pin, HIGH);
- delayMicroseconds(5);
- digitalWrite(motor_speed_pin, LOW);
-}
-
-inline void extruder::temperature_error()
-{
- Serial.print("E: ");
- Serial.println(get_temperature());
-}
-
-//warmup if we're too cold; cool down if we're too hot
-inline void extruder::wait_for_temperature()
-{
-/*
- if(wait_till_cool())
- {
- temperature_error();
- return;
- }
-*/
- if(wait_till_hot())
- temperature_error();
-}
-
-inline void extruder::set_direction(bool dir)
-{
- e_direction = dir;
- digitalWrite(motor_dir_pin, e_direction);
-}
-
-inline void extruder::set_cooler(byte sp)
-{
- if(step_en_pin >= 0) // Step enable conflicts with the fan
- return;
- analogWrite(fan_pin, sp);
-}
-
-/**********************************************************************************************
-
-* RepRap Motherboard v 1.x (x >= 1)
-* Extruder is now on RS485
-
-*/
-
-#else
-
-class extruder
-{
-private:
-
- byte address;
-
-public:
- extruder(byte a);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline extruder::extruder(byte a)
-{
- address = a;
-}
-
-inline void extruder::wait_for_temperature()
-{
-
-}
-
-inline void extruder::valve_set(bool open, int dTime)
-{
- if(open)
- talker.sendPacket(address, "V1");
- else
- talker.sendPacket(address, "V0");
- delay(dTime);
-}
-
-inline void extruder::set_direction(bool direction)
-{
-
-}
-
-inline void extruder::set_cooler(byte e_speed)
-{
-
-}
-
-inline void extruder::set_temperature(int temp)
-{
-
-}
-
-inline int extruder::get_temperature()
-{
- return 1;
-}
-
-inline void extruder::manage()
-{
-
-}
-
-inline void extruder::step()
-{
-
-}
-
-inline void extruder::enableStep()
-{
-
-}
-
-inline void extruder::disableStep()
-{
-
-}
-
-#endif
-
-extern extruder* ex[];
-extern byte extruder_in_use;
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h.dist b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h.dist
deleted file mode 100644
index 2ab8a8cc..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.h.dist
+++ /dev/null
@@ -1,230 +0,0 @@
-
-// Class for controlling each extruder
-//
-// Adrian Bowyer 14 May 2009
-
-#ifndef EXTRUDER_H
-#define EXTRUDER_H
-
-#define EXTRUDER_COUNT 2
-
-void manage_all_extruders();
-
-void new_extruder(byte e);
-
-/**********************************************************************************************
-
-* Sanguino/RepRap Motherboard v 1.0
-
-*/
-
-#if MOTHERBOARD < 2
-
-#define EXTRUDER_FORWARD true
-#define EXTRUDER_REVERSE false
-
-class extruder
-{
-private:
-
-//these our the default values for the extruder.
- byte e_speed;
- int target_celsius;
- int max_celsius;
- byte heater_low;
- byte heater_high;
- byte heater_current;
- int extrude_step_count;
-
-// These are used for temperature control
- byte count ;
- int oldT, newT;
-
-//this is for doing encoder based extruder control
-// int rpm;
-// long e_delay;
-// int error;
-// int last_extruder_error;
-// int error_delta;
- bool e_direction;
- bool valve_open;
-
-// The pins we control
- byte motor_dir_pin, motor_speed_pin, heater_pin, fan_pin, temp_pin, valve_dir_pin, valve_en_pin, step_en_pin;
-
- byte wait_till_hot();
- //byte wait_till_cool();
- void temperature_error();
- int sample_temperature();
-
-public:
-
- extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
-// void set_speed(float es);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
-// Interrupt setup and handling functions for stepper-driven extruders
-
-// void interrupt();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline void extruder::enableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, ENABLE_ON);
-}
-
-inline void extruder::disableStep()
-{
- if(step_en_pin < 0)
- return;
- digitalWrite(step_en_pin, !ENABLE_ON);
-}
-
-inline void extruder::step()
-{
- digitalWrite(motor_speed_pin, HIGH);
- delayMicroseconds(5);
- digitalWrite(motor_speed_pin, LOW);
-}
-
-inline void extruder::temperature_error()
-{
- Serial.print("E: ");
- Serial.println(get_temperature());
-}
-
-//warmup if we're too cold; cool down if we're too hot
-inline void extruder::wait_for_temperature()
-{
-/*
- if(wait_till_cool())
- {
- temperature_error();
- return;
- }
-*/
- if(wait_till_hot())
- temperature_error();
-}
-
-inline void extruder::set_direction(bool dir)
-{
- e_direction = dir;
- digitalWrite(motor_dir_pin, e_direction);
-}
-
-inline void extruder::set_cooler(byte sp)
-{
- if(step_en_pin >= 0) // Step enable conflicts with the fan
- return;
- analogWrite(fan_pin, sp);
-}
-
-/**********************************************************************************************
-
-* RepRap Motherboard v 1.x (x >= 1)
-* Extruder is now on RS485
-
-*/
-
-#else
-
-class extruder
-{
-private:
-
- byte address;
-
-public:
- extruder(byte a);
- void wait_for_temperature();
- void valve_set(bool open, int dTime);
- void set_direction(bool direction);
- void set_cooler(byte e_speed);
- void set_temperature(int temp);
- int get_temperature();
- void manage();
- void step();
-
- void enableStep();
- void disableStep();
-
-};
-
-inline extruder::extruder(byte a)
-{
- address = a;
-}
-
-inline void extruder::wait_for_temperature()
-{
-
-}
-
-inline void extruder::valve_set(bool open, int dTime)
-{
- if(open)
- talker.sendPacket(address, "V1");
- else
- talker.sendPacket(address, "V0");
- delay(dTime);
-}
-
-inline void extruder::set_direction(bool direction)
-{
-
-}
-
-inline void extruder::set_cooler(byte e_speed)
-{
-
-}
-
-inline void extruder::set_temperature(int temp)
-{
-
-}
-
-inline int extruder::get_temperature()
-{
- return 1;
-}
-
-inline void extruder::manage()
-{
-
-}
-
-inline void extruder::step()
-{
-
-}
-
-inline void extruder::enableStep()
-{
-
-}
-
-inline void extruder::disableStep()
-{
-
-}
-
-#endif
-
-extern extruder* ex[];
-extern byte extruder_in_use;
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.pde
deleted file mode 100644
index ce4593fd..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/extruder.pde
+++ /dev/null
@@ -1,308 +0,0 @@
-
-#include "parameters.h"
-#include "pins.h"
-#include "ThermistorTable.h"
-#include "extruder.h"
-
-// Keep all extruders up to temperature etc.
-
-void manage_all_extruders()
-{
- for(byte i = 0; i < EXTRUDER_COUNT; i++)
- ex[i]->manage();
-}
-
-// Select a new extruder
-
-void new_extruder(byte e)
-{
- if(e < 0)
- e = 0;
- if(e >= EXTRUDER_COUNT)
- e = EXTRUDER_COUNT - 1;
-
- if(e != extruder_in_use)
- {
- extruder_in_use = e;
- //setExtruder();
- }
-}
-
-/***************************************************************************************************************************
-
-If we have a new motherboard (V 1.x, x >= 1), the extruder is entirely controlled via the RS485, and all the functions to do
-it are simple inlines in extruder.h
-
-Otherwise, we have to do the work ourselves...
-*/
-
-#if MOTHERBOARD < 2
-extruder::extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin)
-{
- motor_dir_pin = md_pin;
- motor_speed_pin = ms_pin;
- heater_pin = h_pin;
- fan_pin = f_pin;
- temp_pin = t_pin;
- valve_dir_pin = vd_pin;
- valve_en_pin = ve_pin;
- step_en_pin = se_pin;
-
- //setup our pins
- pinMode(motor_dir_pin, OUTPUT);
- pinMode(motor_speed_pin, OUTPUT);
- pinMode(heater_pin, OUTPUT);
-
- pinMode(temp_pin, INPUT);
- pinMode(valve_dir_pin, OUTPUT);
- pinMode(valve_en_pin, OUTPUT);
-
- //initialize values
- digitalWrite(motor_dir_pin, EXTRUDER_FORWARD);
-
- analogWrite(heater_pin, 0);
- analogWrite(motor_speed_pin, 0);
- digitalWrite(valve_dir_pin, false);
- digitalWrite(valve_en_pin, 0);
-
-// The step enable pin and the fan pin are the same...
-// We can have one, or the other, but not both
-
- if(step_en_pin >= 0)
- {
- pinMode(step_en_pin, OUTPUT);
- disableStep();
- } else
- {
- pinMode(fan_pin, OUTPUT);
- analogWrite(fan_pin, 0);
- }
-
- //these our the default values for the extruder.
- e_speed = 0;
- target_celsius = 0;
- max_celsius = 0;
- heater_low = 64;
- heater_high = 255;
- heater_current = 0;
- valve_open = false;
-
-//this is for doing encoder based extruder control
-// rpm = 0;
-// e_delay = 0;
-// error = 0;
-// last_extruder_error = 0;
-// error_delta = 0;
- e_direction = EXTRUDER_FORWARD;
-
- //default to cool
- set_temperature(target_celsius);
-}
-
-
-byte extruder::wait_till_hot()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() < target_celsius - HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT > oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-
-/*
-byte extruder::wait_till_cool()
-{
- count = 0;
- oldT = get_temperature();
- while (get_temperature() > target_celsius + HALF_DEAD_ZONE)
- {
- manage_all_extruders();
- count++;
- if(count > 20)
- {
- newT = get_temperature();
- if(newT < oldT)
- oldT = newT;
- else
- return 1;
- count = 0;
- }
- delay(1000);
- }
- return 0;
-}
-*/
-
-
-
-void extruder::valve_set(bool open, int dTime)
-{
- wait_for_temperature();
- valve_open = open;
- digitalWrite(valve_dir_pin, open);
- digitalWrite(valve_en_pin, 1);
- delay(dTime);
- digitalWrite(valve_en_pin, 0);
-}
-
-
-void extruder::set_temperature(int temp)
-{
- target_celsius = temp;
- max_celsius = (temp*11)/10;
-
- // If we've turned the heat off, we might as well disable the extrude stepper
- // if(target_celsius < 1)
- // disableStep();
-}
-
-/**
-* Samples the temperature and converts it to degrees celsius.
-* Returns degrees celsius.
-*/
-int extruder::get_temperature()
-{
-#ifdef USE_THERMISTOR
- int raw = sample_temperature(temp_pin);
-
- int celsius = 0;
- byte i;
-
- for (i=1; i<NUMTEMPS; i++)
- {
- if (temptable[i][0] > raw)
- {
- celsius = temptable[i-1][1] +
- (raw - temptable[i-1][0]) *
- (temptable[i][1] - temptable[i-1][1]) /
- (temptable[i][0] - temptable[i-1][0]);
-
- break;
- }
- }
-
- // Overflow: Set to last value in the table
- if (i == NUMTEMPS) celsius = temptable[i-1][1];
- // Clamp to byte
- if (celsius > 255) celsius = 255;
- else if (celsius < 0) celsius = 0;
-
- return celsius;
-#else
- return ( 5.0 * sample_temperature() * 100.0) / 1024.0;
-#endif
-}
-
-
-
-/*
-* This function gives us an averaged sample of the analog temperature pin.
-*/
-int extruder::sample_temperature()
-{
- int raw = 0;
-
- //read in a certain number of samples
- for (byte i=0; i<TEMPERATURE_SAMPLES; i++)
- raw += analogRead(temp_pin);
-
- //average the samples
- raw = raw/TEMPERATURE_SAMPLES;
-
- //send it back.
- return raw;
-}
-
-/*!
- Manages extruder functions to keep temps, speeds etc
- at the set levels. Should be called only by manage_all_extruders(),
- which should be called in all non-trivial loops.
- o If temp is too low, don't start the motor
- o Adjust the heater power to keep the temperature at the target
- */
-void extruder::manage()
-{
- //make sure we know what our temp is.
- int current_celsius = get_temperature();
- byte newheat = 0;
-
- //put the heater into high mode if we're not at our target.
- if (current_celsius < target_celsius)
- newheat = heater_high;
- //put the heater on low if we're at our target.
- else if (current_celsius < max_celsius)
- newheat = heater_low;
-
- // Only update heat if it changed
- if (heater_current != newheat) {
- heater_current = newheat;
- analogWrite(heater_pin, heater_current);
- }
-}
-
-
-#if 0
-void extruder::set_speed(float sp)
-{
- // DC motor?
- if(step_en_pin < 0)
- {
- e_speed = (byte)sp;
- if(e_speed > 0)
- wait_for_temperature();
- analogWrite(motor_speed_pin, e_speed);
- return;
- }
-
- // No - stepper
- disableTimerInterrupt();
-
- if(sp <= 1.0e-4)
- {
- disableStep();
- e_speed = 0; // Just use this as a flag
- return;
- } else
- {
- wait_for_temperature();
- enableStep();
- e_speed = 1;
- }
-
- extrude_step_count = 0;
-
- float milliseconds_per_step = 60000.0/(E_STEPS_PER_MM*sp);
- long thousand_ticks_per_step = 4*(long)(milliseconds_per_step);
- setupTimerInterrupt();
- setTimer(thousand_ticks_per_step);
- enableTimerInterrupt();
-}
-
-void extruder::interrupt()
-{
- if(!e_speed)
- return;
- extrude_step_count++;
- if(extrude_step_count > 1000)
- {
- step();
- extrude_step_count = 0;
- }
-}
-
-#endif
-#endif
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.h
deleted file mode 100644
index fbfd83d2..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#ifndef INTERCOM_H
-#define INTERCOM_H
-
-#if MOTHERBOARD > 1
-
-#define IC_BUFFER 10
-#define MASTER_ADDRESS "00"
-
-//our RS485 pins
-#define RX_ENABLE_PIN 13
-#define TX_ENABLE_PIN 12
-
-
-class intercom
-{
- private:
- char myBuffer[IC_BUFFER];
- bool ok;
- void getPacket(char* string, int len);
-
- public:
- intercom();
- void sendPacket(byte address, char* string);
- void sendPacketWithReply(byte address, char* string, char* reply);
-
-};
-
-extern intercom talker;
-
-#endif
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.pde
deleted file mode 100644
index 5832ae5c..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/intercom.pde
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Class to handle internal communications in the machine via RS485
- *
- * Adrian Bowyer 3 July 2009
- *
- */
-
-#if MOTHERBOARD > 1
-
-#include "intercom.h"
-
-intercom::intercom()
-{
- pinMode(RX_ENABLE_PIN, OUTPUT);
- pinMode(TX_ENABLE_PIN, OUTPUT);
- digitalWrite(RX_ENABLE_PIN, 0); //always listen.
- Serial1.begin(38400);
-}
-
-void intercom::sendPacket(byte address, char* string)
-{
- digitalWrite(TX_ENABLE_PIN, 1);
- Serial1.print(address, HEX);
- Serial1.print(MASTER_ADDRESS);
- Serial1.println(string);
- digitalWrite(TX_ENABLE_PIN, 0);
- getPacket(myBuffer, IC_BUFFER);
- if(myBuffer[0] != MASTER_ADDRESS[0] || myBuffer[1] != MASTER_ADDRESS[1])
- {
- // Horrible error - what to do?
- }
-}
-
-void intercom::sendPacketWithReply(byte address, char* string, char* reply)
-{
-
-}
-
-void intercom::getPacket(char* string, int len)
-{
- int i = -1;
- ok = true;
- do
- {
- while(!Serial1.available()) delay(1);
- i++;
-
- // Stop runaway buffer overflow
-
- if(i >= len)
- ok = false;
- else
- string[i] = Serial1.read();
- } while(string[i] != '\n' && ok);
- string[i] = 0;
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h
deleted file mode 100644
index 1ffba5be..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h
+++ /dev/null
@@ -1,106 +0,0 @@
-#ifndef PARAMETERS_H
-#define PARAMETERS_H
-
-// Here are the moterboard codes; set MOTHERBOARD to the right one
-// (Arduino: 0 - no longer in use)
-// Sanguino or RepRap Motherboard with direct drive extruders: 1
-// RepRap Motherboard with RS485 extruders: 2
-// Arduino Mega: 3
-
-#define MOTHERBOARD 1
-
-// Set 1s where you have endstops; 0s where you don't
-#define ENDSTOPS_MIN_ENABLED 1
-#define ENDSTOPS_MAX_ENABLED 0
-
-//our command string length
-#define COMMAND_SIZE 128
-
-// The size of the movement buffer
-
-#define BUFFER_SIZE 4
-
-// Number of microseconds between timer interrupts when no movement
-// is happening
-
-#define DEFAULT_TICK (long)1000
-
-// What delay() value to use when waiting for things to free up in milliseconds
-
-#define WAITING_DELAY 1
-
-#define INCHES_TO_MM 25.4
-
-// define the parameters of our machine.
-#define X_STEPS_PER_MM 7.99735
-#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM)
-#define X_MOTOR_STEPS 400
-#define INVERT_X_DIR 0
-
-#define Y_STEPS_PER_MM 7.99735
-#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM)
-#define Y_MOTOR_STEPS 400
-#define INVERT_Y_DIR 0
-
-#define Z_STEPS_PER_MM 320
-#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM)
-#define Z_MOTOR_STEPS 400
-#define INVERT_Z_DIR 0
-
-// For when we have a stepper-driven extruder
-// E_STEPS_PER_MM is the number of steps needed to
-// extrude 1mm out of the nozzle.
-
-#define E_STEPS_PER_MM 0.706 // 5mm diameter drive - NEMA17 pinch extruder
-//#define E_STEPS_PER_MM 2.206 // 8mm geared drive - NEMA14 pinch extruder
-#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM)
-#define E_MOTOR_STEPS 400
-
-//our maximum feedrates
-#define FAST_XY_FEEDRATE 3000.0
-#define FAST_Z_FEEDRATE 50.0
-
-// Data for acceleration calculations
-// Comment out the next line to turn accelerations off
-#define ACCELERATION_ON
-#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating
-#define SLOW_Z_FEEDRATE 20
-
-// Set to 1 if enable pins are inverting
-// For RepRap stepper boards version 1.x the enable pins are *not* inverting.
-// For RepRap stepper boards version 2.x and above the enable pins are inverting.
-#define INVERT_ENABLE_PINS 1
-
-#if INVERT_ENABLE_PINS == 1
-#define ENABLE_ON LOW
-#else
-#define ENABLE_ON HIGH
-#endif
-
-// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)
-// RepRap opto endstops are *not* inverting.
-#define ENDSTOPS_INVERTING 0
-
-// Inline interrupt control functions
-
-inline void enableTimerInterrupt()
-{
- TIMSK1 |= (1<<OCIE1A);
-}
-
-inline void disableTimerInterrupt()
-{
- TIMSK1 &= ~(1<<OCIE1A);
-}
-
-inline void setTimerCeiling(unsigned int c)
-{
- OCR1A = c;
-}
-
-inline void resetTimer()
-{
- TCNT2 = 0;
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h.dist b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h.dist
deleted file mode 100644
index 5a3a43e3..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/parameters.h.dist
+++ /dev/null
@@ -1,105 +0,0 @@
-#ifndef PARAMETERS_H
-#define PARAMETERS_H
-
-// Here are the moterboard codes; set MOTHERBOARD to the right one
-// (Arduino: 0 - no longer in use)
-// Sanguino or RepRap Motherboard with direct drive extruders: 1
-// RepRap Motherboard with RS485 extruders: 2
-// Arduino Mega: 3
-
-#define MOTHERBOARD 1
-
-// Set 1s where you have endstops; 0s where you don't
-#define ENDSTOPS_MIN_ENABLED 1
-#define ENDSTOPS_MAX_ENABLED 0
-
-//our command string length
-#define COMMAND_SIZE 128
-
-// The size of the movement buffer
-
-#define BUFFER_SIZE 4
-
-// Number of microseconds between timer interrupts when no movement
-// is happening
-
-#define DEFAULT_TICK (long)1000
-
-// What delay() value to use when waiting for things to free up in milliseconds
-
-#define WAITING_DELAY 1
-
-#define INCHES_TO_MM 25.4
-
-// define the parameters of our machine.
-#define X_STEPS_PER_MM 7.99735
-#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM)
-#define X_MOTOR_STEPS 400
-#define INVERT_X_DIR 0
-
-#define Y_STEPS_PER_MM 7.99735
-#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM)
-#define Y_MOTOR_STEPS 400
-#define INVERT_Y_DIR 0
-
-#define Z_STEPS_PER_MM 320
-#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM)
-#define Z_MOTOR_STEPS 400
-#define INVERT_Z_DIR 0
-
-// For when we have a stepper-driven extruder
-// E_STEPS_PER_MM is the number of steps needed to
-// extrude 1mm out of the nozzle.
-
-#define E_STEPS_PER_MM 0.706 // 5mm diameter drive - empirically adjusted
-#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM)
-#define E_MOTOR_STEPS 400
-
-//our maximum feedrates
-#define FAST_XY_FEEDRATE 3000.0
-#define FAST_Z_FEEDRATE 50.0
-
-// Data for acceleration calculations
-// Comment out the next line to turn accelerations off
-#define ACCELERATION_ON
-#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating
-#define SLOW_Z_FEEDRATE 20
-
-// Set to 1 if enable pins are inverting
-// For RepRap stepper boards version 1.x the enable pins are *not* inverting.
-// For RepRap stepper boards version 2.x and above the enable pins are inverting.
-#define INVERT_ENABLE_PINS 1
-
-#if INVERT_ENABLE_PINS == 1
-#define ENABLE_ON LOW
-#else
-#define ENABLE_ON HIGH
-#endif
-
-// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)
-// RepRap opto endstops are *not* inverting.
-#define ENDSTOPS_INVERTING 0
-
-// Inline interrupt control functions
-
-inline void enableTimerInterrupt()
-{
- TIMSK1 |= (1<<OCIE1A);
-}
-
-inline void disableTimerInterrupt()
-{
- TIMSK1 &= ~(1<<OCIE1A);
-}
-
-inline void setTimerCeiling(unsigned int c)
-{
- OCR1A = c;
-}
-
-inline void resetTimer()
-{
- TCNT2 = 0;
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h
deleted file mode 100644
index dd48064a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h
+++ /dev/null
@@ -1,142 +0,0 @@
-#ifndef PINS_H
-#define PINS_H
-
-#if MOTHERBOARD == 0
-
-#error The Arduino cannot run the 5D GCode interpreter
-
-/****************************************************************************************
-* Arduino pin assignment - left here as they might be useful
-*
-****************************************************************************************/
-
-#define X_STEP_PIN (byte)2
-#define X_DIR_PIN (byte)3
-#define X_MIN_PIN (byte)4
-#define X_MAX_PIN (byte)9
-
-#define Y_STEP_PIN (byte)10
-#define Y_DIR_PIN (byte)7
-#define Y_MIN_PIN (byte)8
-#define Y_MAX_PIN (byte)13
-
-#define Z_STEP_PIN (byte)19
-#define Z_DIR_PIN (byte)18
-#define Z_MIN_PIN (byte)17
-#define Z_MAX_PIN (byte)16
-
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)11
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)12
-#define EXTRUDER_0_HEATER_PIN (byte)6
-#define EXTRUDER_0_FAN_PIN (byte)5
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)0 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)16 //NB: Conflicts with Max Z!!!!
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)15
-#define EXTRUDER_0_STEP_ENABLE_PIN 5 // 5 - NB conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* Sanguino/RepRap Motherboard with direct-drive extruders
-*
-****************************************************************************************/
-#elif MOTHERBOARD == 1
-
-#define X_STEP_PIN (byte)15
-#define X_DIR_PIN (byte)18
-#define X_MIN_PIN (byte)20
-#define X_MAX_PIN (byte)21
-#define X_ENABLE_PIN (byte)19
-
-#define Y_STEP_PIN (byte)23
-#define Y_DIR_PIN (byte)22
-#define Y_MIN_PIN (byte)25
-#define Y_MAX_PIN (byte)26
-#define Y_ENABLE_PIN (byte)19
-
-#define Z_STEP_PIN (byte)29
-#define Z_DIR_PIN (byte)30
-#define Z_MIN_PIN (byte)2
-#define Z_MAX_PIN (byte)1
-#define Z_ENABLE_PIN (byte)31
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)12
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)16
-#define EXTRUDER_0_HEATER_PIN (byte)14
-#define EXTRUDER_0_FAN_PIN (byte)3
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)4 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)17
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this
-#define EXTRUDER_0_STEP_ENABLE_PIN (byte)3 // 3 - Conflicts with the fan; set -ve if no stepper
-
-#define EXTRUDER_1_MOTOR_SPEED_PIN (byte)4
-#define EXTRUDER_1_MOTOR_DIR_PIN (byte)0
-#define EXTRUDER_1_HEATER_PIN (byte)24
-#define EXTRUDER_1_FAN_PIN (byte)7
-#define EXTRUDER_1_TEMPERATURE_PIN (byte)3 // Analogue input
-#define EXTRUDER_1_VALVE_DIR_PIN (byte) 6
-#define EXTRUDER_1_VALVE_ENABLE_PIN (byte)5 // Valve needs to be redesigned not to need this
-#define EXTRUDER_1_STEP_ENABLE_PIN (byte)-1 // 7 - Conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* RepRap Motherboard with RS485 extruders
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 2
-
-//x axis pins
-#define X_STEP_PIN 15
-#define X_DIR_PIN 18
-#define X_ENABLE_PIN 19
-#define X_MIN_PIN 20
-#define X_MAX_PIN 21
-
-//y axis pins
-#define Y_STEP_PIN 23
-#define Y_DIR_PIN 22
-#define Y_ENABLE_PIN 24
-#define Y_MIN_PIN 25
-#define Y_MAX_PIN 26
-
-//z axis pins
-#define Z_STEP_PIN 27
-#define Z_DIR_PIN 28
-#define Z_ENABLE_PIN 29
-#define Z_MIN_PIN 30
-#define Z_MAX_PIN 31
-
-//our pin for debugging.
-#define DEBUG_PIN 0
-
-//our SD card pins
-#define SD_CARD_WRITE 2
-#define SD_CARD_DETECT 3
-#define SD_CARD_SELECT 4
-
-//our RS485 pins
-#define TX_ENABLE_PIN 12
-#define RX_ENABLE_PIN 13
-
-//pin for controlling the PSU.
-#define PS_ON_PIN 14
-
-/****************************************************************************************
-* Arduino Mega pin assignment
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 3
-
-#error The Arduino Mega is not yet implemented for the 5D GCode interpreter
-
-#else
-
-#error Unknown MOTHERBOARD value in parameters.h
-
-#endif
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h.dist b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h.dist
deleted file mode 100644
index f8720bd1..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/pins.h.dist
+++ /dev/null
@@ -1,142 +0,0 @@
-#ifndef PINS_H
-#define PINS_H
-
-#if MOTHERBOARD == 0
-
-#error The Arduino cannot run the 5D GCode interpreter
-
-/****************************************************************************************
-* Arduino pin assignment - left here as they might be useful
-*
-****************************************************************************************/
-
-#define X_STEP_PIN (byte)2
-#define X_DIR_PIN (byte)3
-#define X_MIN_PIN (byte)4
-#define X_MAX_PIN (byte)9
-
-#define Y_STEP_PIN (byte)10
-#define Y_DIR_PIN (byte)7
-#define Y_MIN_PIN (byte)8
-#define Y_MAX_PIN (byte)13
-
-#define Z_STEP_PIN (byte)19
-#define Z_DIR_PIN (byte)18
-#define Z_MIN_PIN (byte)17
-#define Z_MAX_PIN (byte)16
-
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)11
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)12
-#define EXTRUDER_0_HEATER_PIN (byte)6
-#define EXTRUDER_0_FAN_PIN (byte)5
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)0 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)16 //NB: Conflicts with Max Z!!!!
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)15
-#define EXTRUDER_0_STEP_ENABLE_PIN 5 // 5 - NB conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* Sanguino/RepRap Motherboard with direct-drive extruders
-*
-****************************************************************************************/
-#elif MOTHERBOARD == 1
-
-#define X_STEP_PIN (byte)15
-#define X_DIR_PIN (byte)18
-#define X_MIN_PIN (byte)20
-#define X_MAX_PIN (byte)21
-#define X_ENABLE_PIN (byte)19
-
-#define Y_STEP_PIN (byte)23
-#define Y_DIR_PIN (byte)22
-#define Y_MIN_PIN (byte)25
-#define Y_MAX_PIN (byte)26
-#define Y_ENABLE_PIN (byte)19
-
-#define Z_STEP_PIN (byte)29
-#define Z_DIR_PIN (byte)30
-#define Z_MIN_PIN (byte)1
-#define Z_MAX_PIN (byte)2
-#define Z_ENABLE_PIN (byte)31
-
-//extruder pins
-#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)12
-#define EXTRUDER_0_MOTOR_DIR_PIN (byte)16
-#define EXTRUDER_0_HEATER_PIN (byte)14
-#define EXTRUDER_0_FAN_PIN (byte)3
-#define EXTRUDER_0_TEMPERATURE_PIN (byte)4 // Analogue input
-#define EXTRUDER_0_VALVE_DIR_PIN (byte)17
-#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this
-#define EXTRUDER_0_STEP_ENABLE_PIN (byte)3 // 3 - Conflicts with the fan; set -ve if no stepper
-
-#define EXTRUDER_1_MOTOR_SPEED_PIN (byte)4
-#define EXTRUDER_1_MOTOR_DIR_PIN (byte)0
-#define EXTRUDER_1_HEATER_PIN (byte)24
-#define EXTRUDER_1_FAN_PIN (byte)7
-#define EXTRUDER_1_TEMPERATURE_PIN (byte)3 // Analogue input
-#define EXTRUDER_1_VALVE_DIR_PIN (byte) 6
-#define EXTRUDER_1_VALVE_ENABLE_PIN (byte)5 // Valve needs to be redesigned not to need this
-#define EXTRUDER_1_STEP_ENABLE_PIN (byte)-1 // 7 - Conflicts with the fan; set -ve if no stepper
-
-
-/****************************************************************************************
-* RepRap Motherboard with RS485 extruders
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 2
-
-//x axis pins
-#define X_STEP_PIN 15
-#define X_DIR_PIN 18
-#define X_ENABLE_PIN 19
-#define X_MIN_PIN 20
-#define X_MAX_PIN 21
-
-//y axis pins
-#define Y_STEP_PIN 23
-#define Y_DIR_PIN 22
-#define Y_ENABLE_PIN 24
-#define Y_MIN_PIN 25
-#define Y_MAX_PIN 26
-
-//z axis pins
-#define Z_STEP_PIN 27
-#define Z_DIR_PIN 28
-#define Z_ENABLE_PIN 29
-#define Z_MIN_PIN 30
-#define Z_MAX_PIN 31
-
-//our pin for debugging.
-#define DEBUG_PIN 0
-
-//our SD card pins
-#define SD_CARD_WRITE 2
-#define SD_CARD_DETECT 3
-#define SD_CARD_SELECT 4
-
-//our RS485 pins
-#define TX_ENABLE_PIN 12
-#define RX_ENABLE_PIN 13
-
-//pin for controlling the PSU.
-#define PS_ON_PIN 14
-
-/****************************************************************************************
-* Arduino Mega pin assignment
-*
-****************************************************************************************/
-
-#elif MOTHERBOARD == 3
-
-#error The Arduino Mega is not yet implemented for the 5D GCode interpreter
-
-#else
-
-#error Unknown MOTHERBOARD value in parameters.h
-
-#endif
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/process_g_code.pde b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/process_g_code.pde
deleted file mode 100644
index 941c180a..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/process_g_code.pde
+++ /dev/null
@@ -1,525 +0,0 @@
-
-#include "parameters.h"
-#include "pins.h"
-#include "extruder.h"
-#include "vectors.h"
-#include "cartesian_dda.h"
-
-/* bit-flags for commands and parameters */
-#define GCODE_G (1<<0)
-#define GCODE_M (1<<1)
-#define GCODE_P (1<<2)
-#define GCODE_X (1<<3)
-#define GCODE_Y (1<<4)
-#define GCODE_Z (1<<5)
-#define GCODE_I (1<<6)
-#define GCODE_J (1<<7)
-#define GCODE_K (1<<8)
-#define GCODE_F (1<<9)
-#define GCODE_S (1<<10)
-#define GCODE_Q (1<<11)
-#define GCODE_R (1<<12)
-#define GCODE_E (1<<13)
-#define GCODE_T (1<<14)
-
-
-#define PARSE_INT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_int(str, &val, &seen, flag); \
- break;
-
-#define PARSE_FLOAT(ch, str, len, val, seen, flag) \
- case ch: \
- len = scan_float(str, &val, &seen, flag); \
- break;
-
-/* gcode line parse results */
-struct GcodeParser
-{
- unsigned int seen;
- int G;
- int M;
- int T;
- float P;
- float X;
- float Y;
- float Z;
- float E;
- float I;
- float J;
- float F;
- float S;
- float R;
- float Q;
-};
-
-
-//our command string
-char cmdbuffer[COMMAND_SIZE];
-char c = '?';
-byte serial_count = 0;
-boolean comment = false;
-FloatPoint fp;
-FloatPoint sp;
-
-// The following three inline functions are used for things like return to 0
-
-inline void specialMoveX(const float& x, const float& feed)
-{
- sp = where_i_am;
- sp.x = x;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveY(const float& y, const float& feed)
-{
- sp = where_i_am;
- sp.y = y;
- sp.f = feed;
- qMove(sp);
-}
-
-inline void specialMoveZ(const float& z, const float& feed)
-{
- sp = where_i_am;
- sp.z = z;
- sp.f = feed;
- qMove(sp);
-}
-
-//our feedrate variables.
-//float feedrate = SLOW_XY_FEEDRATE;
-
-/* keep track of the last G code - this is the command mode to use
- * if there is no command in the current string
- */
-int last_gcode_g = -1;
-
-boolean abs_mode = true; //0 = incremental; 1 = absolute
-
-float extruder_speed = 0;
-
-int scan_int(char *str, int *valp);
-int scan_float(char *str, float *valp);
-
-GcodeParser gc; /* string parse result */
-
-
-//init our string processing
-inline void init_process_string()
-{
- serial_count = 0;
- comment = false;
-}
-
-// Get a command and process it
-
-void get_and_do_command()
-{
- //read in characters if we got them.
- if (Serial.available())
- {
- c = Serial.read();
- if(c == '\r')
- c = '\n';
- // Throw away control chars except \n
- if(c >= ' ' || c == '\n')
- {
-
- //newlines are ends of commands.
- if (c != '\n')
- {
- // Start of comment - ignore any bytes received from now on
- if (c == ';')
- comment = true;
-
- // If we're not in comment mode, add it to our array.
- if (!comment)
- cmdbuffer[serial_count++] = c;
- }
-
- }
- }
-
- // Data runaway?
- if(serial_count >= COMMAND_SIZE)
- init_process_string();
-
- //if we've got a real command, do it
- if (serial_count && c == '\n')
- {
- // Terminate string
- cmdbuffer[serial_count] = 0;
-
- //process our command!
- process_string(cmdbuffer, serial_count);
-
- //clear command.
- init_process_string();
-
- // Say we're ready for the next one
-
- if(debugstring[0] != 0)
- {
- Serial.print("ok ");
- Serial.println(debugstring);
- debugstring[0] = 0;
- } else
- Serial.println("ok");
- }
-}
-
-
-
-int parse_string(struct GcodeParser * gc, char instruction[ ], int size)
-{
- int ind;
- int len; /* length of parameter argument */
-
- gc->seen = 0;
-
- len=0;
- /* scan the string for commands and parameters, recording the arguments for each,
- * and setting the seen flag for each that is seen
- */
- for (ind=0; ind<size; ind += (1+len))
- {
- len = 0;
- switch (instruction[ind])
- {
- PARSE_INT('G', &instruction[ind+1], len, gc->G, gc->seen, GCODE_G);
- PARSE_INT('M', &instruction[ind+1], len, gc->M, gc->seen, GCODE_M);
- PARSE_INT('T', &instruction[ind+1], len, gc->T, gc->seen, GCODE_T);
- PARSE_FLOAT('S', &instruction[ind+1], len, gc->S, gc->seen, GCODE_S);
- PARSE_FLOAT('P', &instruction[ind+1], len, gc->P, gc->seen, GCODE_P);
- PARSE_FLOAT('X', &instruction[ind+1], len, gc->X, gc->seen, GCODE_X);
- PARSE_FLOAT('Y', &instruction[ind+1], len, gc->Y, gc->seen, GCODE_Y);
- PARSE_FLOAT('Z', &instruction[ind+1], len, gc->Z, gc->seen, GCODE_Z);
- PARSE_FLOAT('I', &instruction[ind+1], len, gc->I, gc->seen, GCODE_I);
- PARSE_FLOAT('J', &instruction[ind+1], len, gc->J, gc->seen, GCODE_J);
- PARSE_FLOAT('F', &instruction[ind+1], len, gc->F, gc->seen, GCODE_F);
- PARSE_FLOAT('R', &instruction[ind+1], len, gc->R, gc->seen, GCODE_R);
- PARSE_FLOAT('Q', &instruction[ind+1], len, gc->Q, gc->seen, GCODE_Q);
- PARSE_FLOAT('E', &instruction[ind+1], len, gc->E, gc->seen, GCODE_E);
- default:
- break;
- }
- }
-}
-
-
-//Read the string and execute instructions
-void process_string(char instruction[], int size)
-{
- //the character / means delete block... used for comments and stuff.
- if (instruction[0] == '/')
- return;
-
- float fr;
-
- fp.x = 0.0;
- fp.y = 0.0;
- fp.z = 0.0;
- fp.e = 0.0;
- fp.f = 0.0;
-
- //get all our parameters!
- parse_string(&gc, instruction, size);
- /* if no command was seen, but parameters were, then use the last G code as
- * the current command
- */
- if ((!(gc.seen & (GCODE_G | GCODE_M | GCODE_T))) &&
- ((gc.seen != 0) &&
- (last_gcode_g >= 0))
- )
- {
- /* yes - so use the previous command with the new parameters */
- gc.G = last_gcode_g;
- gc.seen |= GCODE_G;
- }
- //did we get a gcode?
- if (gc.seen & GCODE_G)
- {
- last_gcode_g = gc.G; /* remember this for future instructions */
- fp = where_i_am;
- if (abs_mode)
- {
- if (gc.seen & GCODE_X)
- fp.x = gc.X;
- if (gc.seen & GCODE_Y)
- fp.y = gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z = gc.Z;
- if (gc.seen & GCODE_E)
- fp.e = gc.E;
- }
- else
- {
- if (gc.seen & GCODE_X)
- fp.x += gc.X;
- if (gc.seen & GCODE_Y)
- fp.y += gc.Y;
- if (gc.seen & GCODE_Z)
- fp.z += gc.Z;
- if (gc.seen & GCODE_E)
- fp.e += gc.E;
- }
-
- // Get feedrate if supplied - feedrates are always absolute???
- if ( gc.seen & GCODE_F )
- fp.f = gc.F;
-
- // Process the buffered move commands first
- // If we get one, return immediately
-
- switch (gc.G)
- {
- //Rapid move
- case 0:
- fr = fp.f;
- fp.f = FAST_XY_FEEDRATE;
- qMove(fp);
- fp.f = fr;
- return;
-
- // Controlled move
- case 1:
- qMove(fp);
- return;
-
- //go home.
- case 28:
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x - 5, FAST_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 250, FAST_XY_FEEDRATE);
- where_i_am.x = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveX(where_i_am.x + 1, SLOW_XY_FEEDRATE);
- specialMoveX(where_i_am.x - 10, SLOW_XY_FEEDRATE);
- where_i_am.x = 0;
-
- specialMoveY(where_i_am.y - 5, FAST_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 250, FAST_XY_FEEDRATE);
- where_i_am.y = 0;
- where_i_am.f = SLOW_XY_FEEDRATE;
- specialMoveY(where_i_am.y + 1, SLOW_XY_FEEDRATE);
- specialMoveY(where_i_am.y - 10, SLOW_XY_FEEDRATE);
- where_i_am.y = 0;
-
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z - 0.5, FAST_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 250, FAST_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_Z_FEEDRATE;
- specialMoveZ(where_i_am.z + 1, SLOW_Z_FEEDRATE);
- specialMoveZ(where_i_am.z - 2, SLOW_Z_FEEDRATE);
- where_i_am.z = 0;
- where_i_am.f = SLOW_XY_FEEDRATE; // Most sensible feedrate to leave it in
-
- return;
-
-
- default:
- break;
- }
-
- // Non-buffered G commands
- // Wait till the buffer q is empty first
-
- while(!qEmpty()) delay(WAITING_DELAY);
- switch (gc.G)
- {
-
- //Dwell
- case 4:
- delay((int)(gc.P + 0.5));
- break;
-
- //Inches for Units
- case 20:
- setUnits(false);
- break;
-
- //mm for Units
- case 21:
- setUnits(true);
- break;
-
- //Absolute Positioning
- case 90:
- abs_mode = true;
- break;
-
- //Incremental Positioning
- case 91:
- abs_mode = false;
- break;
-
- //Set position as fp
- case 92:
- setPosition(fp);
- break;
-
- default:
- Serial.print("huh? G");
- Serial.println(gc.G, DEC);
- }
- }
-
-
-
-
- //find us an m code.
- if (gc.seen & GCODE_M)
- {
- // Wait till the q is empty first
- while(!qEmpty()) delay(WAITING_DELAY);
-
- switch (gc.M)
- {
- //TODO: this is a bug because search_string returns 0. gotta fix that.
- case 0:
- break;
- /*
- case 0:
- //todo: stop program
- break;
-
- case 1:
- //todo: optional stop
- break;
-
- case 2:
- //todo: program end
- break;
- */
-
-// Now, with E codes, there is no longer any idea of turning the extruder on or off.
-// (But see valve on/off below.)
-
-/*
- //turn extruder on, forward
- case 101:
- ex[extruder_in_use]->set_direction(1);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder on, reverse
- case 102:
- ex[extruder_in_use]->set_direction(0);
- ex[extruder_in_use]->set_speed(extruder_speed);
- break;
-
- //turn extruder off
- case 103:
- ex[extruder_in_use]->set_speed(0);
- break;
-*/
- //custom code for temperature control
- case 104:
- if (gc.seen & GCODE_S)
- {
- ex[extruder_in_use]->set_temperature((int)gc.S);
- }
- break;
-
- //custom code for temperature reading
- case 105:
- Serial.print("T:");
- Serial.println(ex[extruder_in_use]->get_temperature());
- break;
-
- //turn fan on
- case 106:
- ex[extruder_in_use]->set_cooler(255);
- break;
-
- //turn fan off
- case 107:
- ex[extruder_in_use]->set_cooler(0);
- break;
-/*
-// Extruder speed is now entirely controlled by E codes
- //set max extruder speed, 0-255 PWM
- case 108:
- if (gc.seen & GCODE_S)
- extruder_speed = gc.S;
- break;
-*/
-
-// The valve (real, or virtual...) is now the way to control any extruder (such as
-// a pressurised paste extruder) that cannot move using E codes.
-
- // Open the valve
- case 126:
- ex[extruder_in_use]->valve_set(true, (int)(gc.P + 0.5));
- break;
-
- // Close the valve
- case 127:
- ex[extruder_in_use]->valve_set(false, (int)(gc.P + 0.5));
- break;
-
-
- default:
- Serial.print("Huh? M");
- Serial.println(gc.M, DEC);
- }
-
-
-
- }
-
-// Tool (i.e. extruder) change?
-
- if (gc.seen & GCODE_T)
- {
- while(!qEmpty()) delay(WAITING_DELAY);
- new_extruder(gc.T);
- }
-}
-
-int scan_float(char *str, float *valp, unsigned int *seen, unsigned int flag)
-{
- float res;
- int len;
- char *end;
-
- res = (float)strtod(str, &end);
-
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-int scan_int(char *str, int *valp, unsigned int *seen, unsigned int flag)
-{
- int res;
- int len;
- char *end;
-
- res = (int)strtol(str, &end, 10);
- len = end - str;
-
- if (len > 0)
- {
- *valp = res;
- *seen |= flag;
- }
- else
- *valp = 0;
-
- return len; /* length of number */
-}
-
-
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/upload-copy b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/upload-copy
deleted file mode 100755
index 7383a7a1..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/upload-copy
+++ /dev/null
@@ -1,7 +0,0 @@
-#!/bin/sh
-cp cartesian_dda.h cartesian_dda.h.dist
-cp extruder.h extruder.h.dist
-cp parameters.h parameters.h.dist
-cp pins.h pins.h.dist
-cp vectors.h vectors.h.dist
-
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h
deleted file mode 100644
index 938cfe0c..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h
+++ /dev/null
@@ -1,130 +0,0 @@
-// Structs to hold points in 5D space.
-
-
-#ifndef VECTORS_H
-#define VECTORS_H
-
-#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
-
-// Real-world units
-struct FloatPoint
-{
- float x; // Coordinate axes
- float y;
- float z;
- float e; // Extrude length
- float f; // Feedrate
-};
-
-inline FloatPoint operator+(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline FloatPoint operator-(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-// NB - the next gives neither the scalar nor the vector product
-
-inline FloatPoint operator*(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x * b.x;
- result.y = a.y * b.y;
- result.z = a.z * b.z;
- result.e = a.e * b.e;
- result.f = a.f * b.f;
- return result;
-}
-
-// Can't use fabs for this as it's defined somewhere in a #define
-
-inline FloatPoint fabsv(const FloatPoint& a)
-{
- FloatPoint result;
- result.x = fabs(a.x);
- result.y = fabs(a.y);
- result.z = fabs(a.z);
- result.e = fabs(a.e);
- result.f = fabs(a.f);
- return result;
-}
-
-
-// Integer numbers of steps
-struct LongPoint
-{
- long x; // Coordinates
- long y;
- long z;
- long e; // Extrusion
- long f; // Feedrate
-};
-
-inline LongPoint operator+(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline LongPoint operator-(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-inline LongPoint absv(const LongPoint& a)
-{
- LongPoint result;
- result.x = abs(a.x);
- result.y = abs(a.y);
- result.z = abs(a.z);
- result.e = abs(a.e);
- result.f = abs(a.f);
- return result;
-}
-
-
-inline LongPoint roundv(const FloatPoint& a)
-{
- LongPoint result;
- result.x = round(a.x);
- result.y = round(a.y);
- result.z = round(a.z);
- result.e = round(a.e);
- result.f = round(a.f);
- return result;
-}
-
-inline LongPoint to_steps(const FloatPoint& units, const FloatPoint& position)
-{
- return roundv(units*position);
-}
-
-#endif
diff --git a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h.dist b/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h.dist
deleted file mode 100644
index 938cfe0c..00000000
--- a/trunk/darwin/firmware/FiveD_GCode/FiveD_GCode_Interpreter/vectors.h.dist
+++ /dev/null
@@ -1,130 +0,0 @@
-// Structs to hold points in 5D space.
-
-
-#ifndef VECTORS_H
-#define VECTORS_H
-
-#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
-
-// Real-world units
-struct FloatPoint
-{
- float x; // Coordinate axes
- float y;
- float z;
- float e; // Extrude length
- float f; // Feedrate
-};
-
-inline FloatPoint operator+(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline FloatPoint operator-(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-// NB - the next gives neither the scalar nor the vector product
-
-inline FloatPoint operator*(const FloatPoint& a, const FloatPoint& b)
-{
- FloatPoint result;
- result.x = a.x * b.x;
- result.y = a.y * b.y;
- result.z = a.z * b.z;
- result.e = a.e * b.e;
- result.f = a.f * b.f;
- return result;
-}
-
-// Can't use fabs for this as it's defined somewhere in a #define
-
-inline FloatPoint fabsv(const FloatPoint& a)
-{
- FloatPoint result;
- result.x = fabs(a.x);
- result.y = fabs(a.y);
- result.z = fabs(a.z);
- result.e = fabs(a.e);
- result.f = fabs(a.f);
- return result;
-}
-
-
-// Integer numbers of steps
-struct LongPoint
-{
- long x; // Coordinates
- long y;
- long z;
- long e; // Extrusion
- long f; // Feedrate
-};
-
-inline LongPoint operator+(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x + b.x;
- result.y = a.y + b.y;
- result.z = a.z + b.z;
- result.e = a.e + b.e;
- result.f = a.f + b.f;
- return result;
-}
-
-inline LongPoint operator-(const LongPoint& a, const LongPoint& b)
-{
- LongPoint result;
- result.x = a.x - b.x;
- result.y = a.y - b.y;
- result.z = a.z - b.z;
- result.e = a.e - b.e;
- result.f = a.f - b.f;
- return result;
-}
-
-
-inline LongPoint absv(const LongPoint& a)
-{
- LongPoint result;
- result.x = abs(a.x);
- result.y = abs(a.y);
- result.z = abs(a.z);
- result.e = abs(a.e);
- result.f = abs(a.f);
- return result;
-}
-
-
-inline LongPoint roundv(const FloatPoint& a)
-{
- LongPoint result;
- result.x = round(a.x);
- result.y = round(a.y);
- result.z = round(a.z);
- result.e = round(a.e);
- result.f = round(a.f);
- return result;
-}
-
-inline LongPoint to_steps(const FloatPoint& units, const FloatPoint& position)
-{
- return roundv(units*position);
-}
-
-#endif