diff options
author | rrphooky <rrphooky@cb376a5e-1013-0410-a455-b6b1f9ac8223> | 2009-06-14 21:10:26 +0000 |
---|---|---|
committer | rrphooky <rrphooky@cb376a5e-1013-0410-a455-b6b1f9ac8223> | 2009-06-14 21:10:26 +0000 |
commit | f476371354aa5752c5e34429e7c7a39d74680000 (patch) | |
tree | 59a383d5dc74b27f71924c5f15ebe2e8094449a4 | |
parent | 2f4811fdfabd6ec26abf77ba90df81d405f96260 (diff) | |
download | reprap-backup-f476371354aa5752c5e34429e7c7a39d74680000.tar.gz reprap-backup-f476371354aa5752c5e34429e7c7a39d74680000.zip |
tagging 1.1 release
git-svn-id: https://reprap.svn.sourceforge.net/svnroot/reprap@3151 cb376a5e-1013-0410-a455-b6b1f9ac8223
58 files changed, 10089 insertions, 0 deletions
diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ArduinoSlaveExtruder.pde b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ArduinoSlaveExtruder.pde new file mode 100644 index 00000000..d954b78e --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ArduinoSlaveExtruder.pde @@ -0,0 +1,89 @@ +// Yep, this is actually -*- c++ -*- +/********************************************************************************************************* + * RepRap 3rd Generation Firmware (R3G) + * + * Slave Extruder Firmware for Extruder Controller v2.x + * + * Board documentation at: http://make.rrrf.org/ec-2.0 + * Specification for this protocol is located at: http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + * + * Version History: + * + * 0001: Initial release of the protocol and firmware. + * + *********************************************************************************************************/ + +#ifndef __AVR_ATmega168__ +#error Oops! Make sure you have 'Arduino' selected from the boards menu. +#endif + +//include some basic libraries. +#include <WProgram.h> +#include <Servo.h> +#include <SimplePacket.h> +#include <stdint.h> + +#include "Configuration.h" +#include "Datatypes.h" +#include "RS485.h" +#include "Variables.h" +#include "ThermistorTable.h" + +//this is our firmware version +#define FIRMWARE_VERSION 0001 + +//set up our firmware for actual usage. +void setup() +{ + //setup our firmware to a default state. + init_serial(); //dont want to re-initialize serial! + initialize(); + + //this is a simple text string that identifies us. + //Serial.print("R3G Slave v"); + //Serial.println(FIRMWARE_VERSION, DEC); +} + +//this function takes us back to our default state. +void initialize() +{ + is_tool_paused = false; + init_extruder(); +} + +//start our hardware serial drivers +void init_serial() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + digitalWrite(RX_ENABLE_PIN, LOW); //always listen + + Serial.begin(SERIAL_SPEED); +} + +//handle various things we're required to do. +void loop() +{ + //check for and handle any packets that come in. + process_packets(); + + //did we trigger a reversal? + if (motor1_reversal_state) + reverse_motor_1(); + + //manage our extruder stuff. + if (!is_tool_paused) + manage_temperature(); +} + +//handle the abortion of a print job +void abort_print() +{ + //TODO: shut down all things + + //initalize everything to the beginning + initialize(); +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Configuration.h.dist b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Configuration.h.dist new file mode 100644 index 00000000..346e0b82 --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Configuration.h.dist @@ -0,0 +1,90 @@ +/**************************************************************************************** + * Here's where you define the overall electronics setup for your machine. + ****************************************************************************************/ + +// +// CHOOSE WHICH EXTRUDER YOU'RE USING: +// +//#define EXTRUDER_CONTROLLER_VERSION_2_0 +//#define EXTRUDER_CONTROLLER_VERSION_2_1 +#define EXTRUDER_CONTROLLER_VERSION_2_2 + +#define TEMPERATURE_SAMPLES 5 +#define SERIAL_SPEED 38400 + +//the address for commands to listen to +#define RS485_ADDRESS 0 +#define PACKET_TIMEOUT 100 + +/**************************************************************************************** + * Here's where you define the way your motors are driven. + ****************************************************************************************/ +//PWM +#define MOTOR_STYLE 0 +//ENCODER +//#define MOTOR_STYLE 1 +//STEPPER +//define MOTOR_STYLE 2 + +//do you want to reverse the motor? +#define DELAY_FOR_STOP 5 +#define MOTOR_REVERSE_DURATION 300 +#define MOTOR_FORWARD_DURATION 300 + +/**************************************************************************************** + * Here's where you define the speed PID behavior for an encoder + ****************************************************************************************/ +//#define INVERT_QUADRATURE +#define MIN_SPEED 50 //minimum PWM speed to use +#define MAX_SPEED 255 //maximum PWM speed to use +#define SPEED_ERROR_MARGIN 10 //our error margin (to prevent constant seeking) +#define SPEED_INITIAL_PGAIN 1 //our proportional gain. +#define SPEED_INITIAL_IGAIN 100 //our integral gain. +#define SPEED_INITIAL_DGAIN 10 //our derivative gain. + + +/**************************************************************************************** + * Here's where you define the configuration for the stepper. + ****************************************************************************************/ +#define MOTOR_STEPS 200 //number of steps per revolution +#define MOTOR_STEP_MULTIPLIER 2 //step multiplier (full = 1, half=2, etc.) + +/**************************************************************************************** + * Sanguino Pin Assignment + ****************************************************************************************/ + +//these are the pins for the v2.0 Extruder Controller +#if defined(EXTRUDER_CONTROLLER_VERSION_2_0) || defined(EXTRUDER_CONTROLLER_VERSION_2_1) || defined(EXTRUDER_CONTROLLER_VERSION_2_2) + +#define ENCODER_A_PIN 2 +#define ENCODER_B_PIN 3 + +#define RX_ENABLE_PIN 4 +#define TX_ENABLE_PIN 16 + +#define MOTOR_1_SPEED_PIN 5 +#define MOTOR_1_DIR_PIN 7 +#define MOTOR_2_SPEED_PIN 6 +#define MOTOR_2_DIR_PIN 8 + +#define SERVO1_PIN 9 +#define SERVO2_PIN 10 + +#define HEATER_PIN 11 +#define FAN_PIN 12 +#define VALVE_PIN 15 + +#define THERMISTOR_PIN 3 + +#define DEBUG_PIN 13 + +#endif + +//quadrature encoder behavior +#ifdef INVERT_QUADRATURE +#define QUADRATURE_INCREMENT speed_error--; +#define QUADRATURE_DECREMENT speed_error++; +#else +#define QUADRATURE_INCREMENT speed_error++; +#define QUADRATURE_DECREMENT speed_error--; +#endif diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Datatypes.h b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Datatypes.h new file mode 100644 index 00000000..209183bc --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Datatypes.h @@ -0,0 +1,13 @@ +// motor control states. +typedef enum { + MC_PWM = 0, + MC_ENCODER = 1, + MC_STEPPER = 2 +} +MotorControlStyle; + +typedef enum { + MC_FORWARD = 0, + MC_REVERSE = 1 +} +MotorControlDirection; diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Extruder.pde b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Extruder.pde new file mode 100644 index 00000000..783ab45f --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Extruder.pde @@ -0,0 +1,428 @@ +// Yep, this is actually -*- c++ -*- +void init_extruder() +{ + //reset motor1 + motor1_control = MC_PWM; + motor1_dir = MC_FORWARD; + motor1_pwm = 0; + motor1_target_rpm = 0; + motor1_current_rpm = 0; + + //reset motor2 + motor2_control = MC_PWM; + motor2_dir = MC_FORWARD; + motor2_pwm = 0; + motor2_target_rpm = 0; + motor2_current_rpm = 0; + + //free up 9/10 + servo1.detach(); + servo2.detach(); + + //init our PID stuff. + speed_error = 0; + iState = 0; + dState = 0; + pGain = SPEED_INITIAL_PGAIN; + iGain = SPEED_INITIAL_IGAIN; + dGain = SPEED_INITIAL_DGAIN; + + //encoder pins are for reading. + pinMode(ENCODER_A_PIN, INPUT); + pinMode(ENCODER_B_PIN, INPUT); + + //pullups on our encoder pins + digitalWrite(ENCODER_A_PIN, HIGH); + digitalWrite(ENCODER_B_PIN, HIGH); + + //attach our interrupt handler + attachInterrupt(0, read_quadrature, CHANGE); + + //setup our motor control pins. + pinMode(MOTOR_1_SPEED_PIN, OUTPUT); + pinMode(MOTOR_2_SPEED_PIN, OUTPUT); + pinMode(MOTOR_1_DIR_PIN, OUTPUT); + pinMode(MOTOR_2_DIR_PIN, OUTPUT); + + //turn them off and forward. + digitalWrite(MOTOR_1_SPEED_PIN, LOW); + digitalWrite(MOTOR_2_SPEED_PIN, LOW); + digitalWrite(MOTOR_1_DIR_PIN, HIGH); + digitalWrite(MOTOR_2_DIR_PIN, HIGH); + + //setup our various accessory pins. + pinMode(HEATER_PIN, OUTPUT); + pinMode(FAN_PIN, OUTPUT); + pinMode(VALVE_PIN, OUTPUT); + + //turn them all off + digitalWrite(HEATER_PIN, LOW); + digitalWrite(FAN_PIN, LOW); + digitalWrite(VALVE_PIN, LOW); + + //setup our debug pin. + pinMode(DEBUG_PIN, OUTPUT); + digitalWrite(DEBUG_PIN, LOW); + + //default to zero. + set_temperature(0); + + setupTimer1Interrupt(); +} + +void read_quadrature() +{ + // found a low-to-high on channel A + if (digitalRead(ENCODER_A_PIN) == HIGH) + { + // check channel B to see which way + if (digitalRead(ENCODER_B_PIN) == LOW) + QUADRATURE_INCREMENT +else + QUADRATURE_DECREMENT +} +// found a high-to-low on channel A + else + { + // check channel B to see which way + if (digitalRead(ENCODER_B_PIN) == LOW) + QUADRATURE_DECREMENT +else + QUADRATURE_INCREMENT +} +} + +void enable_motor_1() +{ + if (motor1_control == MC_PWM) + { + //nuke any previous reversals. + motor1_reversal_state = false; + + if (motor1_dir == MC_FORWARD) + digitalWrite(MOTOR_1_DIR_PIN, HIGH); + else + digitalWrite(MOTOR_1_DIR_PIN, LOW); + + analogWrite(MOTOR_1_SPEED_PIN, motor1_pwm); + } + else if (motor1_control == MC_ENCODER) + { + speed_error = 0; + disableTimer1Interrupt(); + setTimer1Ticks(motor1_target_rpm); + enableTimer1Interrupt(); + } + else if (motor1_control == MC_STEPPER) + { + setTimer1Ticks(stepper_ticks); + enableTimer1Interrupt(); + } +} + +void disable_motor_1() +{ + if (motor1_control == MC_PWM) + { + analogWrite(MOTOR_1_SPEED_PIN, 0); + + if (motor1_dir == MC_FORWARD) + motor1_reversal_state = true; + } + else if (motor1_control == MC_ENCODER) + { + speed_error = 0; + disableTimer1Interrupt(); + } + else if (motor1_control == MC_STEPPER) + { + disableTimer1Interrupt(); + digitalWrite(MOTOR_1_SPEED_PIN, LOW); + digitalWrite(MOTOR_2_SPEED_PIN, LOW); + } +} + +void reverse_motor_1() +{ + //wait for it to stop. + if (DELAY_FOR_STOP > 0) + cancellable_delay(DELAY_FOR_STOP, 0); + + //reverse our motor for a bit. + if (MOTOR_REVERSE_DURATION > 0 && motor1_reversal_state) + { + digitalWrite(MOTOR_1_DIR_PIN, LOW); + analogWrite(MOTOR_1_SPEED_PIN, motor1_pwm); + cancellable_delay(MOTOR_REVERSE_DURATION, 1); + } + + //wait for it to stop. + if (DELAY_FOR_STOP > 0 && motor1_reversal_state) + cancellable_delay(DELAY_FOR_STOP, 0); + + //forward our motor for a bit. + if (MOTOR_FORWARD_DURATION > 0 && motor1_reversal_state) + { + digitalWrite(MOTOR_1_DIR_PIN, HIGH); + analogWrite(MOTOR_1_SPEED_PIN, motor1_pwm); + cancellable_delay(MOTOR_FORWARD_DURATION, 2); + } + + motor1_reversal_count = 0; + + //finally stop it. + if (motor1_reversal_state) + analogWrite(MOTOR_1_SPEED_PIN, 0); + + //we're done. + motor1_reversal_state = false; +} + +//basically we want to delay unless there is a start command issued. +void cancellable_delay(unsigned int duration, byte state) +{ + if (motor1_reversal_state) + { + for (unsigned int i=0; i<duration; i++) + { + delay(1); + + //keep track of how far we go. + if (state == 1) + motor1_reversal_count++; + else if (state == 2) + motor1_reversal_count--; + + //dont let it go below zero or above our forward duration. + motor1_reversal_count = max(0, motor1_reversal_count); + motor1_reversal_count = min(MOTOR_FORWARD_DURATION, motor1_reversal_count); + + //check for packets. + process_packets(); + + //did we start up? break! + if (!motor1_reversal_state) + break; + } + } +} + +void enable_motor_2() +{ + if (motor2_control == MC_PWM) + { + if (motor2_dir == MC_FORWARD) + digitalWrite(MOTOR_2_DIR_PIN, HIGH); + else + digitalWrite(MOTOR_2_DIR_PIN, LOW); + + analogWrite(MOTOR_2_SPEED_PIN, motor2_pwm); + } + else if (motor2_control == MC_ENCODER) + { + speed_error = 0; + setTimer1Ticks(motor2_target_rpm/16); + enableTimer1Interrupt(); + } +} + +void disable_motor_2() +{ + if (motor2_control == MC_PWM) + analogWrite(MOTOR_2_SPEED_PIN, 0); + else if (motor2_control == MC_ENCODER) + { + speed_error = 0; + disableTimer1Interrupt(); + } +} + +void enable_fan() +{ + digitalWrite(FAN_PIN, HIGH); +} + +void disable_fan() +{ + digitalWrite(FAN_PIN, LOW); +} + +void open_valve() +{ + digitalWrite(VALVE_PIN, HIGH); +} + +void close_valve() +{ + digitalWrite(VALVE_PIN, LOW); +} + +byte is_tool_ready() +{ + //are we within 5% of the temperature? + if (current_temperature > (int)(target_temperature * 0.95)) + return 1; + else + return 0; +} + +void set_temperature(int temp) +{ + target_temperature = temp; + max_temperature = (int)((float)temp * 1.1); +} + +/** + * Samples the temperature and converts it to degrees celsius. + * Returns degrees celsius. + */ +int get_temperature() +{ +#ifdef THERMISTOR_PIN + return read_thermistor(); +#endif +#ifdef THERMOCOUPLE_PIN + return read_thermocouple(); +#endif +} + +/* +* This function gives us the temperature from the thermistor in Celsius + */ +#ifdef THERMISTOR_PIN +int read_thermistor() +{ + int raw = sample_temperature(THERMISTOR_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]); + + if (celsius > 255) + celsius = 255; + + break; + } + } + + // Overflow: We just clamp to 0 degrees celsius + if (i == NUMTEMPS) + celsius = 0; + + return celsius; +} +#endif + +/* +* This function gives us the temperature from the thermocouple in Celsius + */ +#ifdef THERMOCOUPLE_PIN +int read_thermocouple() +{ + return ( 5.0 * sample_temperature(THERMOCOUPLE_PIN) * 100.0) / 1024.0; +} +#endif + +/* +* This function gives us an averaged sample of the analog temperature pin. + */ +int sample_temperature(byte pin) +{ + int raw = 0; + + //read in a certain number of samples + for (byte i=0; i<TEMPERATURE_SAMPLES; i++) + raw += analogRead(pin); + + //average the samples + raw = raw/TEMPERATURE_SAMPLES; + + //send it back. + return raw; +} + + +/*! + Manages motor and heater based on measured temperature: + o If temp is too low, don't start the motor + o Adjust the heater power to keep the temperature at the target + */ +void manage_temperature() +{ + //make sure we know what our temp is. + current_temperature = get_temperature(); + + //put the heater into high mode if we're not at our target. + if (current_temperature < target_temperature) + analogWrite(HEATER_PIN, heater_high); + //put the heater on low if we're at our target. + else if (current_temperature < max_temperature) + analogWrite(HEATER_PIN, heater_low); + //turn the heater off if we're above our max. + else + analogWrite(HEATER_PIN, 0); +} + + +//this handles the timer interrupt event +void manage_motor1_speed() +{ + // somewhat hacked implementation of a PID algorithm as described at: + // http://www.embedded.com/2000/0010/0010feat3.htm - PID Without a PhD, Tim Wescott + + int abs_error = abs(speed_error); + int pTerm = 0; + int iTerm = 0; + int dTerm = 0; + int speed = 0; + + //hack for extruder not keeping up, overflowing, then shutting off. + if (speed_error < -5000) + speed_error = -500; + if (speed_error > 5000) + speed_error = 500; + + if (speed_error < 0) + { + //calculate our P term + pTerm = abs_error / pGain; + + //calculate our I term + iState += abs_error; + iState = constrain(iState, iMin, iMax); + iTerm = iState / iGain; + + //calculate our D term + dTerm = (abs_error - dState) * dGain; + dState = abs_error; + + //calculate our PWM, within bounds. + speed = pTerm + iTerm - dTerm; + } + + //our debug loop checker thingie + /* + cnt++; + if (cnt > 250) + { + Serial.print("e:"); + Serial.println(speed_error); + Serial.print("spd:"); + Serial.println(speed); + cnt = 0; + } + */ + + //figure out our real speed and use it. + motor1_pwm = constrain(speed, MIN_SPEED, MAX_SPEED); + + analogWrite(MOTOR_1_SPEED_PIN, motor1_pwm); +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/PacketProcessor.pde b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/PacketProcessor.pde new file mode 100644 index 00000000..5dc4ef90 --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/PacketProcessor.pde @@ -0,0 +1,290 @@ +// Yep, this is actually -*- c++ -*- +// These are our query commands from the host +#define SLAVE_CMD_VERSION 0 +#define SLAVE_CMD_INIT 1 +#define SLAVE_CMD_GET_TEMP 2 +#define SLAVE_CMD_SET_TEMP 3 +#define SLAVE_CMD_SET_MOTOR_1_PWM 4 +#define SLAVE_CMD_SET_MOTOR_2_PWM 5 +#define SLAVE_CMD_SET_MOTOR_1_RPM 6 +#define SLAVE_CMD_SET_MOTOR_2_RPM 7 +#define SLAVE_CMD_SET_MOTOR_1_DIR 8 +#define SLAVE_CMD_SET_MOTOR_2_DIR 9 +#define SLAVE_CMD_TOGGLE_MOTOR_1 10 +#define SLAVE_CMD_TOGGLE_MOTOR_2 11 +#define SLAVE_CMD_TOGGLE_FAN 12 +#define SLAVE_CMD_TOGGLE_VALVE 13 +#define SLAVE_CMD_SET_SERVO_1_POS 14 +#define SLAVE_CMD_SET_SERVO_2_POS 15 +#define SLAVE_CMD_FILAMENT_STATUS 16 +#define SLAVE_CMD_GET_MOTOR_1_PWM 17 +#define SLAVE_CMD_GET_MOTOR_2_PWM 18 +#define SLAVE_CMD_GET_MOTOR_1_RPM 19 +#define SLAVE_CMD_GET_MOTOR_2_RPM 20 +#define SLAVE_CMD_SELECT_TOOL 21 +#define SLAVE_CMD_IS_TOOL_READY 22 + +//initialize the firmware to default state. +void init_commands() +{ + finishedCommands = 0; + masterPacket.init(); +} + +//handle our packets. +void process_packets() +{ + //do we have any data to process? + if (Serial.available() > 0) + { + unsigned long start = millis(); + unsigned long end = start + PACKET_TIMEOUT; + + //is there serial data available? + //read through our available data + while (!masterPacket.isFinished()) + { + //only try to grab it if theres something in the queue. + if (Serial.available() > 0) + { + digitalWrite(DEBUG_PIN, HIGH); + + //grab a byte and process it. + byte d = Serial.read(); + masterPacket.process_byte(d); + + //keep us goign while we have data coming in. + start = millis(); + end = start + PACKET_TIMEOUT; + + digitalWrite(DEBUG_PIN, LOW); + } + + //are we sure we wanna break mid-packet? + //have we timed out? + if (millis() >= end) + return; + } + + //do we have a finished packet? + if (masterPacket.isFinished()) + { + //only process packets intended for us. + if (masterPacket.get_8(0) == RS485_ADDRESS) + { + //take some action. + handle_query(); + + //send reply over RS485 + send_reply(); + + //how many have we processed? + finishedCommands++; + + //okay, we'll come back later. + return; + } + } + + //always clean up the packet. + masterPacket.init(); + } +} + +void send_reply() +{ + //might be needed to allow for pin switching / etc. testing needed. + delayMicrosecondsInterruptible(50); + + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + + delayMicrosecondsInterruptible(10); + + //okay, send our response + masterPacket.sendReply(); + + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx +} + +//this is for handling query commands that need a response. +void handle_query() +{ + byte temp; + + //which one did we get? + switch (masterPacket.get_8(1)) + { + //WORKING + case SLAVE_CMD_VERSION: + //get our host version + master_version = masterPacket.get_16(2); + + //send our version back. + masterPacket.add_16(FIRMWARE_VERSION); + break; + + //WORKING + case SLAVE_CMD_INIT: + //just initialize + initialize(); + break; + + //WORKING + case SLAVE_CMD_GET_TEMP: + masterPacket.add_16(current_temperature); + break; + + //WORKING + case SLAVE_CMD_SET_TEMP: + target_temperature = masterPacket.get_16(2); + break; + + //WORKING + case SLAVE_CMD_SET_MOTOR_1_PWM: + motor1_control = MC_PWM; + motor1_pwm = masterPacket.get_8(2); + break; + + //WORKING + case SLAVE_CMD_SET_MOTOR_2_PWM: + motor2_control = MC_PWM; + motor2_pwm = masterPacket.get_8(2); + break; + + //NEEDS TESTING + case SLAVE_CMD_SET_MOTOR_1_RPM: + motor1_target_rpm = masterPacket.get_32(2) * 16; + #if MOTOR_STYLE == 1 + motor1_control = MC_ENCODER; + #else + motor1_control = MC_STEPPER; + stepper_ticks = motor1_target_rpm / (MOTOR_STEPS * MOTOR_STEP_MULTIPLIER); + stepper_high_pwm = motor1_pwm; + stepper_low_pwm = round((float)motor1_pwm * 0.4); + #endif + break; + + //NEEDS TESTING + case SLAVE_CMD_SET_MOTOR_2_RPM: + motor2_control = MC_ENCODER; + motor2_target_rpm = masterPacket.get_32(2); + break; + + case SLAVE_CMD_SET_MOTOR_1_DIR: + temp = masterPacket.get_8(2); + if (temp & 1) + motor1_dir = MC_FORWARD; + else + motor1_dir = MC_REVERSE; + break; + + case SLAVE_CMD_SET_MOTOR_2_DIR: + temp = masterPacket.get_8(2); + if (temp & 1) + motor2_dir = MC_FORWARD; + else + motor2_dir = MC_REVERSE; + break; + + case SLAVE_CMD_TOGGLE_MOTOR_1: + temp = masterPacket.get_8(2); + if (temp & 2) + motor1_dir = MC_FORWARD; + else + motor1_dir = MC_REVERSE; + + if (temp & 1) + { + enable_motor_1(); + + //if we interrupted a reversal, wait for the motor to get back to its old position. + if (motor1_reversal_count > 0) + delay(motor1_reversal_count); + motor1_reversal_count = 0; + } + else + disable_motor_1(); + break; + + //WORKING + case SLAVE_CMD_TOGGLE_MOTOR_2: + temp = masterPacket.get_8(2); + if (temp & 2) + motor2_dir = MC_FORWARD; + else + motor2_dir = MC_REVERSE; + + //TODO: check to see if we're not in stepper mode. + if (temp & 1) + enable_motor_2(); + else + disable_motor_2(); + break; + + //WORKING + case SLAVE_CMD_TOGGLE_FAN: + temp = masterPacket.get_8(2); + if (temp & 1) + enable_fan(); + else + disable_fan(); + break; + + //WORKING + case SLAVE_CMD_TOGGLE_VALVE: + temp = masterPacket.get_8(2); + if (temp & 1) + open_valve(); + else + close_valve(); + break; + + //WORKING + case SLAVE_CMD_SET_SERVO_1_POS: + servo1.attach(9); + servo1.write(masterPacket.get_8(2)); + break; + + //WORKING + case SLAVE_CMD_SET_SERVO_2_POS: + servo2.attach(10); + servo2.write(masterPacket.get_8(2)); + break; + + //WORKING + case SLAVE_CMD_FILAMENT_STATUS: + //TODO: figure out how to detect this. + masterPacket.add_8(255); + break; + + //WORKING + case SLAVE_CMD_GET_MOTOR_1_PWM: + masterPacket.add_8(motor1_pwm); + break; + + //WORKING + case SLAVE_CMD_GET_MOTOR_2_PWM: + masterPacket.add_8(motor2_pwm); + break; + + //NEEDS TESTING + case SLAVE_CMD_GET_MOTOR_1_RPM: + masterPacket.add_32(motor1_current_rpm); + break; + + //NEEDS TESTING + case SLAVE_CMD_GET_MOTOR_2_RPM: + masterPacket.add_32(motor1_current_rpm); + break; + + //WORKING + case SLAVE_CMD_SELECT_TOOL: + //do we do anything? + break; + + //WORKING + case SLAVE_CMD_IS_TOOL_READY: + masterPacket.add_8(is_tool_ready()); + break; + } +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/RS485.h b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/RS485.h new file mode 100644 index 00000000..163f3af5 --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/RS485.h @@ -0,0 +1,15 @@ +void rs485_tx(byte b) +{ + Serial.print(b, BYTE); + + //read for our own byte. + long now = millis(); + long end = now + 10; + int tmp = Serial.read(); + while (tmp != b) + { + tmp = Serial.read(); + if (millis() > tmp) + break; + } +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ThermistorTable.h b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ThermistorTable.h new file mode 100644 index 00000000..5e703bfa --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/ThermistorTable.h @@ -0,0 +1,77 @@ +// +// Start of temperature lookup table +// +// Thermistor lookup table for RepRap Temperature Sensor Boards (http://make.rrrf.org/ts) +// 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 +#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 } +}; +// +// End of temperature lookup table +// diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Timer1.pde b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Timer1.pde new file mode 100644 index 00000000..d11972d3 --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Timer1.pde @@ -0,0 +1,156 @@ +// Yep, this is actually -*- c++ -*- +//these routines provide an easy interface for controlling timer1 interrupts + +//this handles the timer interrupt event +SIGNAL(SIG_OUTPUT_COMPARE1A) +{ + #if MOTOR_STYLE == 1 + //do encoder stuff here. + #else + //increment our index + if (motor1_dir == MC_FORWARD) + stepper_index = (stepper_index - 1) % 8; + else + stepper_index = (stepper_index + 1) % 8; + + //coil 1 + digitalWrite(MOTOR_1_DIR_PIN, coil_a_direction & (1 << stepper_index)); + if (coil_a_enabled & (1 << stepper_index)) + analogWrite(MOTOR_1_SPEED_PIN, stepper_high_pwm); + else + analogWrite(MOTOR_1_SPEED_PIN, stepper_low_pwm); + + //coil 2 + digitalWrite(MOTOR_2_DIR_PIN, coil_b_direction & (1 << stepper_index)); + if (coil_b_enabled & (1 << stepper_index)) + analogWrite(MOTOR_2_SPEED_PIN, stepper_high_pwm); + else + analogWrite(MOTOR_2_SPEED_PIN, stepper_low_pwm); + #endif +} + +void enableTimer1Interrupt() +{ + //enable our interrupt! + TIMSK1 |= (1<<OCIE1A); +} + +void disableTimer1Interrupt() +{ + TIMSK1 &= ~(1<<ICIE1); + TIMSK1 &= ~(1<<OCIE1A); +} + +void setTimer1Resolution(byte r) +{ + //from table 15-5 in that atmega168 datasheet: + // we're setting CS12 - CS10 which correspond to the binary numbers 0-5 + // 0 = no timer + // 1 = no prescaler + // 2 = clock/8 + // 3 = clock/64 + // 4 = clock/256 + // 5 = clock/1024 + + if (r > 5) + r = 5; + + TCCR1B &= B11111000; + TCCR1B |= r; +} + +void setTimer1Ceiling(unsigned int c) +{ + OCR1A = c; +} + + +unsigned int getTimer1Ceiling(unsigned long ticks) +{ + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs) + if (ticks <= 65535L) + return (ticks & 0xffff); + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs) + else if (ticks <= 524280L) + return ((ticks / 8) & 0xffff); + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs) + else if (ticks <= 4194240L) + return ((ticks / 64) & 0xffff); + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs) + else if (ticks <= 16776960L) + return (ticks / 256); + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs) + else if (ticks <= 67107840L) + return (ticks / 1024); + //its really slow... hopefully we can just get by with super slow. + else + return 65535; +} + +byte getTimer1Resolution(unsigned long ticks) +{ + // these also represent frequency: 1000000 / ticks / 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 (ticks <= 65535L) + return 1; + // 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 (ticks <= 524280L) + return 2; + // 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 (ticks <= 4194240L) + return 3; + // 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 (ticks <= 16776960L) + return 4; + // 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 (ticks <= 67107840L) + return 5; + //its really slow... hopefully we can just get by with super slow. + else + return 5; +} + +void setTimer1Ticks(unsigned long ticks) +{ + // ticks is the delay between interrupts in 62.5 nanosecond 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. + + setTimer1Ceiling(getTimer1Ceiling(ticks)); + setTimer1Resolution(getTimer1Resolution(ticks)); +} + +void setupTimer1Interrupt() +{ + //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. + setTimer1Resolution(5); + setTimer1Ceiling(65535); + disableTimer1Interrupt(); +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Utils.pde b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Utils.pde new file mode 100644 index 00000000..e09a86ff --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Utils.pde @@ -0,0 +1,22 @@ +void delayMicrosecondsInterruptible(unsigned int us) +{ + // 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; + + // busy wait + __asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles +"brne 1b" : + "=w" (us) : + "0" (us) // 2 cycles + ); +} diff --git a/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Variables.h b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Variables.h new file mode 100644 index 00000000..4192c932 --- /dev/null +++ b/tags/firmware/gen3/1.1/ArduinoSlaveExtruder/Variables.h @@ -0,0 +1,57 @@ +//this is the version of our host software +unsigned int master_version = 0; + +//how many queued commands have we processed? +//this will be used to keep track of our current progress. +unsigned long finishedCommands = 0; + +//are we paused? +boolean is_tool_paused = false; + +int current_temperature = 0; +int target_temperature = 0; +int max_temperature = 0; +int heater_low = 64; +int heater_high = 255; + +MotorControlStyle motor1_control = MC_PWM; +volatile MotorControlDirection motor1_dir = MC_FORWARD; +volatile byte motor1_pwm = 0; +volatile long motor1_target_rpm = 0; +volatile long motor1_current_rpm = 0; +boolean motor1_reversal_state = false; +int motor1_reversal_count = 0; + +MotorControlStyle motor2_control = MC_PWM; +MotorControlDirection motor2_dir = MC_FORWARD; +byte motor2_pwm = 0; +long motor2_target_rpm = 0; +long motor2_current_rpm = 0; + +Servo servo1; +Servo servo2; + +//these are for the extruder PID +volatile int speed_error = 0; // extruder position / error variable. +volatile int pGain = SPEED_INITIAL_PGAIN; // Proportional gain +volatile int iGain = SPEED_INITIAL_IGAIN; // Integral gain +volatile int dGain = SPEED_INITIAL_DGAIN; // Derivative gain +volatile int iMax = 500; // Integrator max +volatile int iMin = -500; // Integrator min +volatile int iState = 0; // Integrator state +volatile int dState = 0; // Last position input + +//these are our packet classes +SimplePacket masterPacket(rs485_tx); + +//variables to keep track of stepper state. +const byte coil_a_enabled = B10011001; +const byte coil_a_direction = B11000011; +const byte coil_b_enabled = B01100110; +const byte coil_b_direction = B11110000; + +//what state are we in? +volatile byte stepper_index = 0; +volatile long stepper_ticks = 0; +volatile byte stepper_high_pwm = 0; +volatile byte stepper_low_pwm = 0; diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/CircularBuffer.h b/tags/firmware/gen3/1.1/SanguinoMaster/CircularBuffer.h new file mode 100644 index 00000000..fa026b8e --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/CircularBuffer.h @@ -0,0 +1,178 @@ +#ifndef _CIRCULAR_BUFFER_H_ +#define _CIRCULAR_BUFFER_H_ +/** + * Sanguino 3rd Generation Firmware (S3G) + * + * Specification for this protocol is located at: + * http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + */ + +#include <stddef.h> +#include <stdint.h> +#include "Timer1.h" + +/** + * This is an implementation of a simple in-memory circular + * buffer. + * + * Please note that this implementation is NOT thread/interrupt + * safe. Remember that 16-bit data access is not atomic on + * 8-bit platforms! Turn off interrupts before accessing the + * buffer. + */ +class CircularBuffer { +private: + uint8_t* buffer; // ptr to the in-memory buffer + uint16_t capacity; // size of the buffer + uint16_t head; // index of first element of data + uint16_t tail; // index of last element of data + uint16_t currentSize; // number of elements of valid data in buffer +public: + /** + * Create a circular buffer of the given size with a provided + * chunk of memory. + * This implementation does not claim ownership of the buffer! + */ + CircularBuffer(uint16_t capacity, uint8_t* pBuf) { + this->capacity = capacity; + buffer = pBuf; + clear(); + } + + ~CircularBuffer() { + } + + /** + * Reset the circular buffer to an empty state. + */ + void clear() { + head = tail = 0; + currentSize = 0; + } + + /** + * Return a byte of data in the circular buffer specified by its index. + */ + uint8_t operator[](uint16_t i) { + uint16_t idx; + idx = (i + tail) % capacity; + return buffer[idx]; + } + + /** + * Return the remaining capacity of the circular buffer. + */ + uint16_t remainingCapacity() + { + uint16_t remaining; + remaining = (capacity - currentSize); + return remaining; + } + + /** + * Return the current number of valid bytes in the buffer. + */ + uint16_t size() { + uint16_t csize; + csize = currentSize; + return csize; + } + +private: + /** + * Internal: append a byte of data to the buffer's contents. + */ + void appendInternal(uint8_t datum) + { + if ((currentSize + 1) <= capacity) + { + buffer[head] = datum; + head = (head + 1) % capacity; + currentSize++; + } + } + +public: + /** + * Append a single byte of data to the buffer. + */ + void append(uint8_t datum) { + appendInternal(datum); + } + + /** + * Append a two-byte value to the buffer. + * The buffer stores values in little-endian format. + */ + void append_16(uint16_t datum) { + appendInternal(datum & 0xff); + appendInternal((datum >> 8) & 0xff); + } + + /** + * Append a four-byte value to the buffer. + * The buffer stores values in little-endian format. + */ + void append_32(uint32_t datum) { + appendInternal(datum & 0xff); + appendInternal((datum >> 8) & 0xff); + appendInternal((datum >> 16) & 0xff); + appendInternal((datum >> 24) & 0xff); + } + +private: + /** + * Internal: Remove the first byte of the buffer's data and return it. + */ + uint8_t removeInternal() { + if (currentSize == 0) + return 0; + else + { + uint8_t c = buffer[tail]; + tail = (tail + 1) % capacity; + currentSize--; + return c; + } + } + +public: + /** + * Remove and return a byte from the start of the circular buffer. + */ + uint8_t remove_8() { + return removeInternal(); + } + + /* + * Remove and return a two-byte value from the start of the circular + * buffer. The buffer stores values in little-endian format. + */ + uint16_t remove_16() { + uint8_t v[2]; + v[0] = removeInternal(); + v[1] = removeInternal(); + return v[0] | ((uint16_t)v[1] << 8); + } + + /* + * Remove and return a four-byte value from the start of the circular + * buffer. The buffer stores values in little-endian format. + */ + uint32_t remove_32() { + uint8_t v[4]; + v[0] = removeInternal(); + v[1] = removeInternal(); + v[2] = removeInternal(); + v[3] = removeInternal(); + return v[0] | + ((uint32_t)v[1] << 8) | + ((uint32_t)v[2] << 16) | + ((uint32_t)v[3] << 24); + } +}; + +#endif // _CIRCULAR_BUFFER_H_ diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Commands.h b/tags/firmware/gen3/1.1/SanguinoMaster/Commands.h new file mode 100644 index 00000000..6eb0e22e --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Commands.h @@ -0,0 +1,74 @@ +#ifndef _COMMANDS_H_ +#define _COMMANDS_H_ +/** + * Sanguino 3rd Generation Firmware (S3G) + * + * Specification for this protocol is located at: + * http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + */ + +/**************************** + *** WARNING *** + **************************** + + * These values here are also used in the replicatorG driver. + * If you modify anything here, please ensure that it is updated + * in the Sanguino3GDriver.java file at: + * https://replicatorg.googlecode.com/svn/trunk/src/replicatorg/app/drivers + */ + +// These are our query commands from the host +#define HOST_CMD_VERSION 0 +#define HOST_CMD_INIT 1 +#define HOST_CMD_GET_BUFFER_SIZE 2 +#define HOST_CMD_CLEAR_BUFFER 3 +#define HOST_CMD_GET_POSITION 4 +#define HOST_CMD_GET_RANGE 5 +#define HOST_CMD_SET_RANGE 6 +#define HOST_CMD_ABORT 7 +#define HOST_CMD_PAUSE 8 +#define HOST_CMD_PROBE 9 +#define HOST_CMD_TOOL_QUERY 10 +#define HOST_CMD_IS_FINISHED 11 + +// These are our bufferable commands from the host +// #define HOST_CMD_QUEUE_POINT_INC 128 // deprecated +#define HOST_CMD_QUEUE_POINT_ABS 129 +#define HOST_CMD_SET_POSITION 130 +#define HOST_CMD_FIND_AXES_MINIMUM 131 +#define HOST_CMD_FIND_AXES_MAXIMUM 132 +#define HOST_CMD_DELAY 133 +#define HOST_CMD_CHANGE_TOOL 134 +#define HOST_CMD_WAIT_FOR_TOOL 135 +#define HOST_CMD_TOOL_COMMAND 136 +#define HOST_CMD_ENABLE_AXES 137 + +// These are our query commands from the host +#define SLAVE_CMD_VERSION 0 +#define SLAVE_CMD_INIT 1 +#define SLAVE_CMD_GET_TEMP 2 +#define SLAVE_CMD_SET_TEMP 3 +#define SLAVE_CMD_SET_MOTOR_1_PWM 4 +#define SLAVE_CMD_SET_MOTOR_2_PWM 5 +#define SLAVE_CMD_SET_MOTOR_1_RPM 6 +#define SLAVE_CMD_SET_MOTOR_2_RPM 7 +#define SLAVE_CMD_SET_MOTOR_1_DIR 8 +#define SLAVE_CMD_SET_MOTOR_2_DIR 9 +#define SLAVE_CMD_TOGGLE_MOTOR_1 10 +#define SLAVE_CMD_TOGGLE_MOTOR_2 11 +#define SLAVE_CMD_TOGGLE_FAN 12 +#define SLAVE_CMD_TOGGLE_VALVE 13 +#define SLAVE_CMD_SET_SERVO_1_POS 14 +#define SLAVE_CMD_SET_SERVO_2_POS 15 +#define SLAVE_CMD_FILAMENT_STATUS 16 +#define SLAVE_CMD_GET_MOTOR_1_PWM 17 +#define SLAVE_CMD_GET_MOTOR_2_PWM 18 +#define SLAVE_CMD_GET_MOTOR_1_RPM 19 +#define SLAVE_CMD_GET_MOTOR_2_RPM 20 +#define SLAVE_CMD_SELECT_TOOL 21 +#define SLAVE_CMD_IS_TOOL_READY 22 + +#endif // _COMMANDS_H_ diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Configuration.h.dist b/tags/firmware/gen3/1.1/SanguinoMaster/Configuration.h.dist new file mode 100755 index 00000000..0e31f04b --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Configuration.h.dist @@ -0,0 +1,157 @@ +/**************************************************************************************** + * Here's where you define the overall electronics setup for your machine. + ****************************************************************************************/ + +// +// CHOOSE WHICH MOTHERBOARD YOU'RE USING: +// +//#define REPRAP_MOTHERBOARD_VERSION_1_0 +//#define REPRAP_MOTHERBOARD_VERSION_1_1 +#define REPRAP_MOTHERBOARD_VERSION_1_2 + +// +// CHOOSE WHICH FAMILY OF STEPPER DRIVER YOU'RE USING: +// +//#define STEPPER_DRIVER_VERSION_1_X +#define STEPPER_DRIVER_VERSION_2_X + +// +// CHOOSE WHICH FAMILY OF OPTO ENDSTOP YOU'RE USING: +// +//#define OPTO_ENDSTOP_1_X +#define OPTO_ENDSTOP_2_X + +#define PACKET_TIMEOUT 1000 +#define HOST_SERIAL_SPEED 38400 +#define SLAVE_SERIAL_SPEED 38400 + +//uncomment to enable debugging functions +#define ENABLE_DEBUG 1 +#define ENABLE_COMMS_DEBUG 1 + +//#define SCAN_TOOLS_ON_STARTUP + +/**************************************************************************************** + * Sanguino Pin Assignment + ****************************************************************************************/ + +//these are the pins for the v1.0 Motherboard. +#ifdef REPRAP_MOTHERBOARD_VERSION_1_0 + +//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 19 +#define Y_MIN_PIN 25 +#define Y_MAX_PIN 26 + +//z axis pins +#define Z_STEP_PIN 29 +#define Z_DIR_PIN 30 +#define Z_ENABLE_PIN 31 +#define Z_MIN_PIN 1 +#define Z_MAX_PIN 2 + +//our pin for debugging. +#define DEBUG_PIN 0 + +//our SD card pins +#define SD_CARD_SELECT 4 +#define SD_CARD_WRITE 28 +#define SD_CARD_DETECT 24 + + +//our RS485 pins +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 + + +#endif + +//these are the pins for the v1.1 Motherboard. +#if defined(REPRAP_MOTHERBOARD_VERSION_1_1) || defined(REPRAP_MOTHERBOARD_VERSION_1_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 + +#endif + +/**************************************************************************************** + * Stepper Driver Behaviour Definition + ****************************************************************************************/ + +//do we want a step delay (ie: length of pulse in microseconds) comment out to disable. +#define STEP_DELAY 1 + +#ifdef STEPPER_DRIVER_VERSION_1_X +#define STEPPER_ENABLE 1 +#define STEPPER_DISABLE 0 +#endif + +#ifdef STEPPER_DRIVER_VERSION_2_X +#define STEPPER_ENABLE 0 +#define STEPPER_DISABLE 1 +#endif + +/**************************************************************************************** + * Opto Endstop Behaviour Definition + ****************************************************************************************/ + +#ifdef OPTO_ENDSTOP_1_X +#define SENSORS_INVERTING 0 +#endif + +#ifdef OPTO_ENDSTOP_2_X +#define SENSORS_INVERTING 1 +#endif + + +/**************************************************************************************** + * Various Buffer Size Declarations + ****************************************************************************************/ +//we store all queueable commands in one big giant buffer. +// Explicitly allocate memory at compile time for buffer. +#define COMMAND_BUFFER_SIZE 256 +#define POINT_QUEUE_SIZE 32 +#define POINT_SIZE 16 // 4 + 4 + 4 + 4 = 16 +#define MAX_PACKET_LENGTH 32 diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Datatypes.h b/tags/firmware/gen3/1.1/SanguinoMaster/Datatypes.h new file mode 100644 index 00000000..2f56b559 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Datatypes.h @@ -0,0 +1,28 @@ +#ifndef _DATATYPES_H_ +#define _DATATYPES_H_ +/** + * Sanguino 3rd Generation Firmware (S3G) + * + * Specification for this protocol is located at: + * http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + */ + +#include <stdint.h> + +/** + * Our point representation. Each coordinate is a 4-byte value. + */ +struct LongPoint { +public: + int32_t x; + int32_t y; + int32_t z; +}; + +#define COMMAND_MODE_IDLE 0 +#define COMMAND_MODE_WAIT_FOR_TOOL 1 + +#endif // _DATATYPES_H_ diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Debug.pde b/tags/firmware/gen3/1.1/SanguinoMaster/Debug.pde new file mode 100644 index 00000000..19fa025b --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Debug.pde @@ -0,0 +1,353 @@ +#ifdef ENABLE_DEBUG + +/* +void square_move() +{ + queue_absolute_point(100, -100, 0, 2, 20833); + Serial.println("Bottom right"); + queue_absolute_point(100, -100, 300, 2, 10000); + Serial.println("Up"); + queue_absolute_point(100, 100, 300, 2, 20833); + Serial.println("Top right"); + queue_absolute_point(-100, 100, 300, 2, 20833); + Serial.println("Top left"); + queue_absolute_point(-100, 100, 0, 2, 10000); + Serial.println("Down"); + queue_absolute_point(-100, -100, 0, 2, 20833); + Serial.println("Bottom left"); +} + +void circle_move() +{ + int x, y; + float rads; + + for (int i=0; i<100; i++) + { + rads = 2*PI*(i/100.0); + + x = 200 * sin(rads); + y = 200 * cos(rads); + + queue_absolute_point(x, y, 0, 1, 60000); + } +} +*/ + +void debug_blink() +{ + digitalWrite(DEBUG_PIN, HIGH); + digitalWrite(DEBUG_PIN, LOW); +} + +void check_tool_version(byte id) +{ + ping_tool(id); + + int version = slavePacket.get_16(1); + + //print_tool(id); + //Serial.print("version: "); + //Serial.println(version); +} + +void exercise_heater() +{ + set_tool_temp(1, 100); + while (1) + { + get_tool_temp(1); + delay(500); + } +} + +void exercise_motors() +{ + boolean dir = true; + + Serial.println("forward"); + Serial.println("up"); + for (int i=0; i<256; i++) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + Serial.println("down"); + for (int i=255; i>=0; i--) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + dir = false; + + Serial.println("forward"); + Serial.println("up"); + for (int i=0; i<256; i++) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + Serial.println("down"); + for (int i=255; i>=0; i--) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } +} + +void print_stats() +{ + Serial.println("Stats:"); + Serial.print("Slave TX Count:"); + Serial.println(rs485_tx_count, DEC); + Serial.print("Slave RX Count:"); + Serial.println(rs485_rx_count, DEC); + Serial.print("Slave Packet Count: "); + Serial.println(rs485_packet_count, DEC); + Serial.print("Slave CRC Errors: "); + Serial.println(slave_crc_errors, DEC); + Serial.print("Slave timeouts: "); + Serial.println(slave_timeouts, DEC); +} + +void print_tool(byte id) +{ + Serial.print("tool #"); + Serial.print(id, DEC); + Serial.print(" "); +} + +void get_tool_temp(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_TEMP); + send_packet(); + + int temp = slavePacket.get_16(1); + print_tool(id); + Serial.print("temp: "); + Serial.println(temp, DEC); +} + +void set_tool_temp(byte id, unsigned int temp) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_TEMP); + slavePacket.add_16(temp); + send_packet(); + + print_tool(id); + Serial.print("set temp to: "); + Serial.println(temp, DEC); +} + +void set_motor1_pwm(byte id, byte pwm) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_MOTOR_1_PWM); + slavePacket.add_8(pwm); + send_packet(); + + //print_tool(id); + //Serial.print("set motor1 pwm to: "); + //Serial.println(pwm, DEC); +} + +void set_motor2_pwm(byte id, byte pwm) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_MOTOR_2_PWM); + slavePacket.add_8(pwm); + send_packet(); + + //print_tool(id); + //Serial.print("set motor2 pwm to: "); + //Serial.println(pwm, DEC); +} + + +void set_motor1_rpm(byte id, int rpm) +{ + +} + +void toggle_motor1(byte id, boolean dir, boolean enable) +{ + byte flags = 0; + + if (enable) + flags += 1; + + if (dir) + flags += 2; + + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_MOTOR_1); + slavePacket.add_8(flags); + send_packet(); +} + +void toggle_motor2(byte id, boolean dir, boolean enable) +{ + byte flags = 0; + + if (enable) + flags += 1; + + if (dir) + flags += 2; + + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_MOTOR_2); + slavePacket.add_8(flags); + send_packet(); +} + +void toggle_fan(byte id, boolean enable) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_FAN); + slavePacket.add_8(enable); + send_packet(); +} + +void toggle_valve(byte id, boolean open) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_VALVE); + slavePacket.add_8(open); + send_packet(); +} + +void get_motor1_pwm(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_MOTOR_1_PWM); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("m1 pwm: "); + Serial.println(temp, DEC); +} + +void get_motor2_pwm(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_MOTOR_2_PWM); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("m2 pwm: "); + Serial.println(temp, DEC); +} + +void set_servo1_position(byte id, byte pos) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_SERVO_1_POS); + slavePacket.add_8(pos); + send_packet(); +} + +void set_servo2_position(byte id, byte pos) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_SERVO_2_POS); + slavePacket.add_8(pos); + send_packet(); +} + +void get_motor1_rpm(byte id) +{ + +} + +void get_filament_status(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_FILAMENT_STATUS); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("filament: "); + Serial.println(temp, DEC); +} + +void test_rs485() +{ + for (byte j=0; j<5; j++) + { + Serial.print("starting round "); + Serial.println(j, DEC); + + for (int i=0; i<=255; i++) + { + set_i(i); + delay(50); + } + + delay(1000); + + for (int i=255; i>=0; i--) + { + set_i(i); + delay(50); + } + + delay(1000); + } + + print_stats(); +} + +void set_i(byte i) +{ + set_motor1_pwm(0, i); + toggle_motor1(0, true, true); + set_motor2_pwm(0, i); + toggle_motor2(0, true, true); + + if (i>127) + { + toggle_fan(0, true); + toggle_valve(0, true); + } + else + { + toggle_fan(0, false); + toggle_valve(0, false); + } +} + +#endif diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/PSU.pde b/tags/firmware/gen3/1.1/SanguinoMaster/PSU.pde new file mode 100644 index 00000000..94a59355 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/PSU.pde @@ -0,0 +1,22 @@ +void init_psu() +{ +#ifdef PS_ON_PIN + pinMode(PS_ON_PIN, OUTPUT); + turn_psu_on(); +#endif +} + +void turn_psu_on() +{ +#ifdef PS_ON_PIN + digitalWrite(PS_ON_PIN, LOW); + delay(2000); //wait for PSU to actually turn on. +#endif +} + +void turn_psu_off() +{ +#ifdef PS_ON_PIN + digitalWrite(PS_ON_PIN, HIGH); +#endif +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/PacketProcessor.pde b/tags/firmware/gen3/1.1/SanguinoMaster/PacketProcessor.pde new file mode 100644 index 00000000..0649fe07 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/PacketProcessor.pde @@ -0,0 +1,360 @@ +// Yep, this is actually -*- c++ -*- + +//initialize the firmware to default state. +inline void init_commands() +{ + finishedCommands = 0; +} + +//handle our packets. +void process_host_packets() +{ + unsigned long start = millis(); + unsigned long end = start + PACKET_TIMEOUT; + +#ifdef ENABLE_COMMS_DEBUG + //Serial.print("IN: "); +#endif + + //do we have a finished packet? + while (!hostPacket.isFinished()) + { + if (Serial.available() > 0) + { + digitalWrite(DEBUG_PIN, HIGH); + + //grab a byte and process it. + byte d = Serial.read(); + hostPacket.process_byte(d); + +#ifdef ENABLE_COMMS_DEBUG + //Serial.print(d, HEX); + //Serial.print(" "); +#endif + serial_rx_count++; + + //keep us goign while we have data coming in. + start = millis(); + end = start + PACKET_TIMEOUT; + + if (hostPacket.getResponseCode() == RC_CRC_MISMATCH) + { + //host_crc_errors++; + +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Host CRC Mismatch"); +#endif + } + + digitalWrite(DEBUG_PIN, LOW); + } + + //are we sure we wanna break mid-packet? + //have we timed out? + if (millis() >= end) + { +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Host timeout"); +#endif + break; + } + } + + if (hostPacket.isFinished()) + { + serial_packet_count++; + + byte b = hostPacket.get_8(0); + // top bit high == bufferable command packet (eg. #128-255) + if (b & 0x80) + { + //do we have room? + if (commandBuffer.remainingCapacity() >= hostPacket.getLength()) + { + //okay, throw it in the buffer. + for (byte i=0; i<hostPacket.getLength(); i++) + commandBuffer.append(hostPacket.get_8(i)); + } + else + { + hostPacket.overflow(); + } + } + // top bit low == reply needed query packet (eg. #0-127) + else + handle_query(b); + } + + //okay, send our response + hostPacket.sendReply(); +} + +//this is for handling query commands that need a response. +void handle_query(byte cmd) +{ + //which one did we get? + switch (cmd) + { + //WORKS + case HOST_CMD_VERSION: + //get our host version + host_version = hostPacket.get_16(1); + + //send our version back. + hostPacket.add_16(FIRMWARE_VERSION); + break; + + //WORKS + case HOST_CMD_INIT: + //just initialize + initialize(); + break; + + case HOST_CMD_GET_BUFFER_SIZE: + //send our remaining buffer size. + hostPacket.add_16(commandBuffer.remainingCapacity()); + break; + + case HOST_CMD_CLEAR_BUFFER: + //just clear it. + commandBuffer.clear(); + break; + + case HOST_CMD_GET_POSITION: + //send our position + hostPacket.add_32(current_steps.x); + hostPacket.add_32(current_steps.y); + hostPacket.add_32(current_steps.z); + hostPacket.add_8(get_endstop_states()); + break; + + case HOST_CMD_GET_RANGE: + //send our range + hostPacket.add_32(range_steps.x); + hostPacket.add_32(range_steps.y); + hostPacket.add_32(range_steps.z); + break; + + case HOST_CMD_SET_RANGE: + //set our range to what the host tells us + range_steps.x = (long)hostPacket.get_32(1); + range_steps.y = (long)hostPacket.get_32(5); + range_steps.z = (long)hostPacket.get_32(9); + + //write it back to eeprom + write_range_to_eeprom(); + break; + + case HOST_CMD_ABORT: + //support a microcontrollers right to choice. + abort_print(); + break; + + case HOST_CMD_PAUSE: + if (is_machine_paused) + { + //unpause our machine. + is_machine_paused = false; + + //unpause our tools + set_tool_pause_state(false); + + //resume stepping. + enable_needed_steppers(); + enableTimer1Interrupt(); + } + else + { + //pause our activity. + is_machine_paused = true; + + //pause our tools + set_tool_pause_state(true); + + //pause stepping + disableTimer1Interrupt(); + disable_steppers(); + } + break; + + case HOST_CMD_PROBE: + //we dont support this yet. + hostPacket.unsupported(); + break; + + //WORKS + case HOST_CMD_TOOL_QUERY: + send_tool_query(); + break; + + case HOST_CMD_IS_FINISHED: + { + bool finished = is_point_buffer_empty() && at_target() && commandBuffer.size() == 0; + hostPacket.add_8(finished?1:0); + break; + } + + default: + hostPacket.unsupported(); + } +} + +//this is for handling buffered commands with no response +void handle_commands() +{ + byte flags = 0; + + long x; + long y; + long z; + unsigned long step_delay; + byte cmd; + + //do we have any commands? + if (commandBuffer.size() > 0) + { + /* + Serial.print("size: "); + Serial.println(commandBuffer.size(), DEC); + */ + + //peek at our command. + cmd = commandBuffer[0]; + + // Do it later if it's a point queueing command and we don't have room yet. + if ((cmd == HOST_CMD_QUEUE_POINT_ABS /* || cmd == HOST_CMD_QUEUE_POINT_INC */) && + pointBuffer.remainingCapacity() < POINT_SIZE) { + return; + } + + //okay, which command are we handling? + cmd = commandBuffer.remove_8(); + + /* + Serial.print("cmd: "); + Serial.println(cmd, DEC); + */ + + switch(cmd) + { + case HOST_CMD_QUEUE_POINT_ABS: + x = (long)commandBuffer.remove_32(); + y = (long)commandBuffer.remove_32(); + z = (long)commandBuffer.remove_32(); + step_delay = (unsigned long)commandBuffer.remove_32(); + + queue_absolute_point(x, y, z, step_delay); + + break; + + case HOST_CMD_SET_POSITION: + wait_until_target_reached(); //dont want to get hasty. + cli(); + target_steps.x = current_steps.x = (long)commandBuffer.remove_32(); + target_steps.y = current_steps.y = (long)commandBuffer.remove_32(); + target_steps.z = current_steps.z = (long)commandBuffer.remove_32(); + sei(); + break; + + case HOST_CMD_FIND_AXES_MINIMUM: + wait_until_target_reached(); //dont want to get hasty. + + //no dda interrupts. + disableTimer1Interrupt(); + + //which ones are we going to? + flags = commandBuffer.remove_8(); + + //find them! + seek_minimums( + flags & 1, + flags & 2, + flags & 4, + commandBuffer.remove_32(), + commandBuffer.remove_16()); + + //turn on point seeking agian. + enableTimer1Interrupt(); + + break; + + case HOST_CMD_FIND_AXES_MAXIMUM: + wait_until_target_reached(); //dont want to get hasty. + + //no dda interrupts. + disableTimer1Interrupt(); + + //find them! + seek_maximums( + flags & 1, + flags & 2, + flags & 4, + commandBuffer.remove_32(), + commandBuffer.remove_16()); + + //turn on point seeking agian. + enableTimer1Interrupt(); + + break; + + case HOST_CMD_DELAY: + wait_until_target_reached(); //dont want to get hasty. + + //take it easy. + delay(commandBuffer.remove_32()); + break; + + case HOST_CMD_CHANGE_TOOL: + wait_until_target_reached(); //dont want to get hasty. + + //extruder, i choose you! + select_tool(commandBuffer.remove_8()); + break; + + case HOST_CMD_WAIT_FOR_TOOL: + wait_until_target_reached(); //dont want to get hasty. + + //get your temp in gear, you lazy bum. + + //what tool / timeout /etc? + currentToolIndex = commandBuffer.remove_8(); + toolPingDelay = (unsigned int)commandBuffer.remove_16(); + toolTimeout = (unsigned int)commandBuffer.remove_16(); + + //check to see if its ready now + if (!is_tool_ready(currentToolIndex)) + { + //how often to ping? + toolNextPing = millis() + toolPingDelay; + toolTimeoutEnd = millis() + (toolTimeout * 1000); + + //okay, put us in ping-tool-until-ready mode + commandMode = COMMAND_MODE_WAIT_FOR_TOOL; + } + break; + + case HOST_CMD_TOOL_COMMAND: + wait_until_target_reached(); //dont want to get hasty. + + send_tool_command(); + break; + case HOST_CMD_ENABLE_AXES: + wait_until_target_reached(); + { + unsigned char param = commandBuffer.remove_8(); + bool x = (param & 0x01) != 0; + bool y = (param & 0x02) != 0; + bool z = (param & 0x04) != 0; + if ((param & 0x80) != 0) { + // enable axes + enable_steppers(x,y,z); + } else { + // disable axes + disable_steppers(x,y,z); + } + } + default: + hostPacket.unsupported(); + } + } +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/RS485.h b/tags/firmware/gen3/1.1/SanguinoMaster/RS485.h new file mode 100644 index 00000000..14c8a778 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/RS485.h @@ -0,0 +1,41 @@ +unsigned long rs485_tx_count = 0; +unsigned long rs485_rx_count = 0; +unsigned long rs485_packet_count = 0; +unsigned long rs485_loopback_fails = 0; +unsigned long slave_crc_errors = 0; +unsigned long slave_timeouts = 0; + +unsigned long serial_tx_count = 0; +unsigned long serial_rx_count = 0; +unsigned long serial_packet_count = 0; + +void rs485_tx(byte b) +{ + rs485_tx_count++; + + Serial1.print(b, BYTE); + + //read for our own byte. + long now = millis(); + long end = now + 10; + int tmp = Serial1.read(); + while (tmp != b) + { + tmp = Serial1.read(); + if (millis() > end) + { + rs485_loopback_fails++; +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Loopback Fail"); +#endif + break; + } + } +} + +void serial_tx(byte b) +{ + serial_tx_count++; + + Serial.print(b, BYTE); +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/SDCard.pde b/tags/firmware/gen3/1.1/SanguinoMaster/SDCard.pde new file mode 100644 index 00000000..01d3c479 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/SDCard.pde @@ -0,0 +1,130 @@ +// Yep, this is actually -*- c++ -*- +#ifdef USE_SD_CARD + +void init_sd_card() +{ + if (!card.init_card()) + { + if (!card.isAvailable()) + { + Serial.println("No card present"); + error = 1; + } + else + { + Serial.println("Card init failed"); + error = 2; + } + } + else if (!card.open_partition()) + { + Serial.println("No partition"); + error = 3; + } + else if (!card.open_filesys()) + { + Serial.println("Can't open filesys"); + error = 4; + } + else if (!card.open_dir("/")) + { + Serial.println("Can't open /"); + error = 5; + } + else if (card.isLocked()) + { + Serial.println("Card is locked"); + error = 6; + } +} + +void open_file() +{ + open_new_file(); + + if (error == 0) + { + bufferIndex = 0; + for (char c = 'a'; c<= 'z'; c++) + { + buffer[bufferIndex] = c; + bufferIndex++; + } + + card.write_file(f, (uint8_t *) buffer, bufferIndex); + card.close_file(f); + } + + read_first_file(); +} + +void open_new_file() +{ + strcpy(buffer, "RRJOB00.TXT"); + for (buffer[5] = '0'; buffer[5] <= '9'; buffer[5]++) + { + for (buffer[6] = '0'; buffer[6] <= '9'; buffer[6]++) + { + f = card.open_file(buffer); + if (!f) + break; // found a file! + card.close_file(f); + } + if (!f) + break; + } + + if(!card.create_file(buffer)) + { + Serial.print("couldnt create: "); + Serial.println(buffer); + error = 7; + } + else + { + f = card.open_file(buffer); + if (!f) + { + Serial.print("error opening: "); + Serial.println(buffer); + error = 8; + } + else + { + Serial.print("writing to: "); + Serial.println(buffer); + } + } +} + +void read_first_file() +{ + strcpy(buffer, "RRJOB00.TXT"); + f = card.open_file(buffer); + + if (!f) + { + Serial.print("error opening: "); + Serial.println(buffer); + error = 8; + } + else + { + Serial.print("reading from: "); + Serial.println(buffer); + + uint8_t result = card.read_file(f, (uint8_t *)buffer, 8); + + if (result >0) + Serial.print(buffer); + else if (result == 0) + Serial.println("end of file."); + else + { + Serial.print("read error: "); + Serial.println(result, DEC); + } + } +} + +#endif diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/SanguinoMaster.pde b/tags/firmware/gen3/1.1/SanguinoMaster/SanguinoMaster.pde new file mode 100755 index 00000000..53c71fb3 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/SanguinoMaster.pde @@ -0,0 +1,101 @@ +// Yep, this is actually -*- c++ -*- +/*************************************************************************************** + * Sanguino 3rd Generation Firmware (S3G) + * + * Specification for this protocol is located at: + * http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + * + * Version History: + * + * 0001: Initial release of the protocol and firmware. + * + ***************************************************************************************/ + +//a check to make sure we're compiling for the right firmware +#ifndef __AVR_ATmega644P__ +#error Oops! Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu. +#endif + +//include some basic libraries. +#include <WProgram.h> +#include <SimplePacket.h> + +#include "Configuration.h" +#include "Datatypes.h" +#include "Timer1.h" +#include "CircularBuffer.h" +#include "RS485.h" +#include "Variables.h" +#include "Commands.h" +#ifdef USE_SD_CARD +#include <RepRapSDCard.h> +#endif + +//this is our firmware version. +#define FIRMWARE_VERSION 0101 + +//set up our firmware for actual usage. +void setup() +{ + //setup our firmware to a default state. + init_serial(); + initialize(); + + //this is a simple text string that identifies us. + Serial.print("R3G Master v"); + Serial.println(FIRMWARE_VERSION, DEC); +} + +//this function takes us back to our default state. +void initialize() +{ + is_machine_paused = false; + + init_psu(); + init_commands(); + init_steppers(); + init_tools(); +} + +//start our hardware serial drivers +void init_serial() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + digitalWrite(RX_ENABLE_PIN, LOW); //always listen. + + Serial.begin(HOST_SERIAL_SPEED); + Serial1.begin(SLAVE_SERIAL_SPEED); +} + +//handle various things we're required to do. +void loop() +{ + //check for and handle any packets that come in. + if (Serial.available()) + process_host_packets(); + + //our basic handling for each loop. + if (commandMode == COMMAND_MODE_IDLE) + handle_commands(); + else if (commandMode == COMMAND_MODE_WAIT_FOR_TOOL) + check_tool_ready_state(); + + check_endstops(); +} + +//handle the abortion of a print job +void abort_print() +{ + //TODO: turn off all of our tools. + + //turn off steppers too. + disableTimer1Interrupt(); + disable_steppers(); + + //initalize everything to the beginning + initialize(); +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Steppers.pde b/tags/firmware/gen3/1.1/SanguinoMaster/Steppers.pde new file mode 100755 index 00000000..c4e285ee --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Steppers.pde @@ -0,0 +1,513 @@ +/** + * Sanguino 3rd Generation Firmware (S3G) + * + * Specification for this protocol is located at: + * http://docs.google.com/Doc?id=dd5prwmp_14ggw37mfp + * + * License: GPLv2 + * Authors: Marius Kintel, Adam Mayer, and Zach Hoeken + */ + + +/// Initialize the stepper driver state. +void init_steppers(); + +/// Move the steppers until the minimum endstop is triggered on the +/// specified axes. All axes are moved at the same rate. +/// \param find_x find the minimum position for the X axis +/// \param find_y find the minimum position for the Y axis +/// \param find_z find the minimum position for the Z axis +/// \param step_delay delay between steps in microseconds +/// \param timeout_seconds seconds to wait before giving up +void seek_minimums(boolean find_x, boolean find_y, boolean find_z, unsigned long step_delay, unsigned int timeout_seconds); +/// Move the axis one step in towards the minimum. If the min endstop is +/// triggered, back up until it is only triggered by a single step. +/// \return true if the minimum endstop is triggered. +boolean find_axis_min(byte step_pin, byte dir_pin, byte min_pin); +void seek_maximums(boolean find_x, boolean find_y, boolean find_z, unsigned long step_delay, unsigned int timeout_seconds); +boolean find_axis_max(byte step_pin, byte dir_pin, byte max_pin); +inline void grab_next_point(); +inline void do_step(byte step_pin); +inline bool read_switch(byte pin); +void check_endstops(); +inline void enable_steppers(bool x, bool y, bool z); +inline void enable_needed_steppers(); +inline void disable_steppers(bool x, bool y, bool z); +inline void disable_steppers(); +byte get_endstop_states(); +void write_range_to_eeprom(); +void read_range_from_eeprom(); +void queue_absolute_point(long x, long y, long z, unsigned long micros); +inline boolean is_point_buffer_empty(); +inline boolean at_target(); +inline void wait_until_target_reached(); + + +//initialize our stepper drivers +void init_steppers() +{ + //clear our point buffer + pointBuffer.clear(); + + //pull in our saved values. + read_range_from_eeprom(); + + //initialize all our pins. + pinMode(X_STEP_PIN, OUTPUT); + pinMode(X_DIR_PIN, OUTPUT); + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(X_MIN_PIN, INPUT); + pinMode(X_MAX_PIN, INPUT); + + pinMode(Y_STEP_PIN, OUTPUT); + pinMode(Y_DIR_PIN, OUTPUT); + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Y_MIN_PIN, INPUT); + pinMode(Y_MAX_PIN, INPUT); + + pinMode(Z_STEP_PIN, OUTPUT); + pinMode(Z_DIR_PIN, OUTPUT); + pinMode(Z_ENABLE_PIN, OUTPUT); + pinMode(Z_MIN_PIN, INPUT); + pinMode(Z_MAX_PIN, INPUT); + +#if SENSORS_INVERTING == 1 + // If we are using inverting endstops, we'll turn on the pull-ups on these pins. + // This enables us to operate without endstops if necessary. + digitalWrite(X_MIN_PIN, HIGH); + digitalWrite(X_MAX_PIN, HIGH); + digitalWrite(Y_MIN_PIN, HIGH); + digitalWrite(Y_MAX_PIN, HIGH); + digitalWrite(Z_MIN_PIN, HIGH); + digitalWrite(Z_MAX_PIN, HIGH); +#endif + + //turn them off to start. + disable_steppers(); + + //zero our deltas. + delta_steps.x = 0; + delta_steps.y = 0; + delta_steps.z = 0; + + //zero our posison. + current_steps.x = 0; + current_steps.y = 0; + current_steps.z = 0; + target_steps.x = 0; + target_steps.y = 0; + target_steps.z = 0; + + //prep timer 1 for handling DDA stuff. + setupTimer1Interrupt(); + setTimer1Micros(2500); + enableTimer1Interrupt(); +} + +void seek_minimums(boolean find_x, boolean find_y, boolean find_z, unsigned long step_delay, unsigned int timeout_seconds) +{ + unsigned long start = millis(); + unsigned long end = millis() + (timeout_seconds*1000); + + enable_steppers(find_x,find_y,find_z); + + boolean found_x = false; + boolean found_y = false; + boolean found_z = false; + + //do it until we time out. + while (millis() < end) + { + //do our steps and check for mins. + if (find_x && !found_x) + { + found_x = find_axis_min(X_STEP_PIN, X_DIR_PIN, X_MIN_PIN); + current_steps.x = 0; + } + if (find_y && !found_y) + { + found_y = find_axis_min(Y_STEP_PIN, Y_DIR_PIN, Y_MIN_PIN); + current_steps.y = 0; + } + if (find_z && !found_z) + { + found_z = find_axis_min(Z_STEP_PIN, Z_DIR_PIN, Z_MIN_PIN); + current_steps.z = 0; + } + + //check to see if we've found all required switches. + if (find_x && !found_x) + true; + else if (find_y && !found_y) + true; + else if (find_z && !found_z) + true; + //found them all. + else + break; + + //do our delay for our axes. + if (step_delay <= 65535) + delayMicrosecondsInterruptible(step_delay); + else + delay(step_delay/1000); + } + disable_steppers(); +} + +void seek_maximums(boolean find_x, boolean find_y, boolean find_z, unsigned long step_delay, unsigned int timeout_seconds) +{ + unsigned long start = millis(); + unsigned long end = millis() + (timeout_seconds*1000); + + enable_steppers(find_x,find_y,find_z); + + boolean found_x = false; + boolean found_y = false; + boolean found_z = false; + + //do it until we time out. + while (millis() < end) + { + //do our steps and check for mins. + if (find_x && !found_x) + { + found_x = find_axis_max(X_STEP_PIN, X_DIR_PIN, X_MAX_PIN); + range_steps.x = current_steps.x; + } + if (find_y && !found_y) + { + found_y = find_axis_max(Y_STEP_PIN, Y_DIR_PIN, Y_MAX_PIN); + range_steps.y = current_steps.y; + } + if (find_z && !found_z) + { + found_z = find_axis_max(Z_STEP_PIN, Z_DIR_PIN, Z_MAX_PIN); + range_steps.z = current_steps.z; + } + + //check to see if we've found all required switches. + if (find_x && !found_x) + true; + else if (find_y && !found_y) + true; + else if (find_z && !found_z) + true; + //found them all. + else + { + write_range_to_eeprom(); + break; + } + + //do our delay for our axes. + if (step_delay <= 65535) + delayMicrosecondsInterruptible(step_delay); + else + delay(step_delay/1000); + } + disable_steppers(); +} + +boolean find_axis_dir(byte step_pin, byte dir_pin, + byte switch_pin, bool maximum) +{ + //are we at the end of our travel? + if (read_switch(switch_pin)) + { + //move slowly in reverse until the switch goes open. + digitalWrite(dir_pin, maximum?LOW:HIGH); + while (read_switch(switch_pin)) + { + do_step(step_pin); + delay(500); + } + //then move one step in the given direction. + digitalWrite(dir_pin, maximum?HIGH:LOW); + do_step(step_pin); + return true; + } + else + { + digitalWrite(dir_pin, maximum?HIGH:LOW); + do_step(step_pin); + } + return false; +} + +boolean find_axis_min(byte step_pin, byte dir_pin, byte min_pin) +{ + return find_axis_dir(step_pin,dir_pin,min_pin,false); +} + +boolean find_axis_max(byte step_pin, byte dir_pin, byte max_pin) +{ + return find_axis_dir(step_pin,dir_pin,max_pin,true); +} + +inline void grab_next_point() +{ + //can we even step to this? + if (pointBuffer.size() >= POINT_SIZE) + { + //whats our target? + target_steps.x = (long)pointBuffer.remove_32(); + target_steps.y = (long)pointBuffer.remove_32(); + target_steps.z = (long)pointBuffer.remove_32(); + + //figure out our deltas + delta_steps.x = target_steps.x - current_steps.x; + delta_steps.y = target_steps.y - current_steps.y; + delta_steps.z = target_steps.z - current_steps.z; + + //what direction? + x_direction = delta_steps.x >= 0; + y_direction = delta_steps.y >= 0; + z_direction = delta_steps.z >= 0; + + //set our direction pins as well + digitalWrite(X_DIR_PIN, x_direction); + digitalWrite(Y_DIR_PIN, y_direction); + digitalWrite(Z_DIR_PIN, z_direction); + + //now get us absolute coords + delta_steps.x = abs(delta_steps.x); + delta_steps.y = abs(delta_steps.y); + delta_steps.z = abs(delta_steps.z); + + //enable our steppers if needed. + enable_needed_steppers(); + + //figure out our deltas + max_delta = 0; + max_delta = max(delta_steps.x, delta_steps.y); + max_delta = max(delta_steps.z, max_delta); + + //init stuff. + x_counter = -max_delta/2; + y_counter = -max_delta/2; + z_counter = -max_delta/2; + + //start the move! + setTimer1Micros(pointBuffer.remove_32()); + enableTimer1Interrupt(); + } + else + is_point_queue_empty = true; //only real place to check. +} + +//do a single step on our DDA line! +SIGNAL(SIG_OUTPUT_COMPARE1A) +{ + check_endstops(); // TODO: this is perhaps inefficient, but necessary. + //increment our x counter, and take steps if required. + if (current_steps.x != target_steps.x) + { + x_counter += delta_steps.x; + + if (x_counter > 0) + { + do_step(X_STEP_PIN); + x_counter -= max_delta; + + if (x_direction) + current_steps.x++; + else + current_steps.x--; + } + } + + //increment our y counter, and take steps if required. + if (current_steps.y != target_steps.y) + { + y_counter += delta_steps.y; + + if (y_counter > 0) + { + do_step(Y_STEP_PIN); + y_counter -= max_delta; + + if (y_direction) + current_steps.y++; + else + current_steps.y--; + } + } + + //increment our z counter, and take steps if required. + if (current_steps.z != target_steps.z) + { + z_counter += delta_steps.z; + + if (z_counter > 0) + { + do_step(Z_STEP_PIN); + z_counter -= max_delta; + + if (z_direction) + current_steps.z++; + else + current_steps.z--; + } + } + + //we're either at our target + if (at_target()) + { +// finishedPoints++; +// Serial.print("Finished:"); +// Serial.println(finishedPoints, DEC); + grab_next_point(); + } +} + +//actually send a step signal. +inline void do_step(byte step_pin) +{ + digitalWrite(step_pin, HIGH); +#ifdef STEP_DELAY + delayMicrosecondsInterruptible(STEP_DELAY); +#endif + digitalWrite(step_pin, LOW); +} + +//figure out if we're at a switch or not +inline bool read_switch(byte pin) +{ + //dual read as crude debounce + if (SENSORS_INVERTING) + return !digitalRead(pin) && !digitalRead(pin); + else + return digitalRead(pin) && digitalRead(pin); +} + +//looks at our endstops and disables our motor if we hit one. +void check_endstops() +{ + if ( (x_direction && read_switch(X_MAX_PIN)) || + (!x_direction && read_switch(X_MIN_PIN)) ) + digitalWrite(X_ENABLE_PIN, STEPPER_DISABLE); + + if ( (y_direction && read_switch(Y_MAX_PIN)) || + (!y_direction && read_switch(Y_MIN_PIN)) ) + digitalWrite(Y_ENABLE_PIN, STEPPER_DISABLE); + + if ( (z_direction && read_switch(Z_MAX_PIN)) || + (!z_direction && read_switch(Z_MIN_PIN)) ) + digitalWrite(Z_ENABLE_PIN, STEPPER_DISABLE); +} + +inline void enable_steppers(bool x, bool y, bool z) +{ + if (x) { digitalWrite(X_ENABLE_PIN, STEPPER_ENABLE); } + if (y) { digitalWrite(Y_ENABLE_PIN, STEPPER_ENABLE); } + if (z) { digitalWrite(Z_ENABLE_PIN, STEPPER_ENABLE); } +} + +// enable our steppers so we can move them. disable any steppers +// not about to be set in motion to reduce power and heat. +// TODO: make this a configuration option (HOLD_AXIS?); there are some +// situations (milling) where you want to leave the steppers on to +// hold the position. +// ZMS: made X/Y axes always on once used. +inline void enable_needed_steppers() +{ + enable_steppers(delta_steps.x > 0, delta_steps.y > 0, delta_steps.z > 0); + if (!(delta_steps.z > 0)) { + disable_steppers(false,false,true); // explicitly turn off Z stepper when not needed + } +} + +inline void disable_steppers(bool x, bool y, bool z) +{ + //disable our steppers + if (x) { digitalWrite(X_ENABLE_PIN, STEPPER_DISABLE); } + if (y) { digitalWrite(Y_ENABLE_PIN, STEPPER_DISABLE); } + if (z) { digitalWrite(Z_ENABLE_PIN, STEPPER_DISABLE); } +} + +//turn off steppers to save juice / keep things cool. +inline void disable_steppers() +{ + //disable our steppers + disable_steppers(true,true,true); +} + +//read all of our states into a single byte. +byte get_endstop_states() +{ + byte state = 0; + + //each one is its own bit in the byte. + state |= read_switch(Z_MAX_PIN) << 5; + state |= read_switch(Z_MIN_PIN) << 4; + state |= read_switch(Y_MAX_PIN) << 3; + state |= read_switch(Y_MIN_PIN) << 2; + state |= read_switch(X_MAX_PIN) << 1; + state |= read_switch(X_MIN_PIN); + + return state; +} + +//TODO: make me work! +void write_range_to_eeprom() +{ + +} + +//TODO: make me work! +void read_range_from_eeprom() +{ + +} + +//queue a point for us to move to +void queue_absolute_point(long x, long y, long z, unsigned long micros) +{ + //wait until we have free space + while (pointBuffer.remainingCapacity() < POINT_SIZE) + delay(1); + + disableTimer1Interrupt(); + + //okay, add in our points. + pointBuffer.append_32(x); + pointBuffer.append_32(y); + pointBuffer.append_32(z); + pointBuffer.append_32(micros); + + //just in case we got interrupted and it changed. + is_point_queue_empty = false; + + enableTimer1Interrupt(); +} + +inline boolean is_point_buffer_empty() +{ + //okay, we got points in the buffer. + if (!is_point_queue_empty) + return false; + + //still working on a point. + //todo: do we need this? + if (!at_target()) + return false; + + //nope, we're done. + return true; +} + +inline boolean at_target() +{ + if (current_steps.x == target_steps.x && current_steps.y == target_steps.y && current_steps.z == target_steps.z) + return true; + else + return false; +} + +inline void wait_until_target_reached() +{ + //todo: check to see if this is what is locking up our code? + while(!is_point_buffer_empty()) + delay(1); +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Timer1.h b/tags/firmware/gen3/1.1/SanguinoMaster/Timer1.h new file mode 100755 index 00000000..ab3571cb --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Timer1.h @@ -0,0 +1,141 @@ +// Yep, this is actually -*- c++ -*- + +#ifndef _TIMER1_H_ +#define _TIMER1_H_ + +//these routines provide an easy interface for controlling timer1 interrupts + +inline void enableTimer1Interrupt() +{ + TIMSK1 |= (1<<OCIE1A); +} + +inline void disableTimer1Interrupt() +{ + TIMSK1 &= ~(1<<OCIE1A); + TIMSK1 &= ~(1<<ICIE1); +} + +inline void setTimer1Resolution(byte r) +{ + //from table 15-5 in that atmega168 datasheet: + // we're setting CS12 - CS10 which correspond to the binary numbers 0-5 + // 0 = no timer + // 1 = no prescaler + // 2 = clock/8 + // 3 = clock/64 + // 4 = clock/256 + // 5 = clock/1024 + + if (r > 5) + r = 5; + + TCCR1B &= B11111000; + TCCR1B |= r; +} + +inline void setTimer1Ceiling(unsigned int c) +{ + OCR1A = c; +} + + +unsigned int getTimer1Ceiling(unsigned long ticks) +{ + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs) + if (ticks <= 65535L) + return (ticks & 0xffff); + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs) + else if (ticks <= 524280L) + return ((ticks / 8) & 0xffff); + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs) + else if (ticks <= 4194240L) + return ((ticks / 64) & 0xffff); + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs) + else if (ticks <= 16776960L) + return (ticks / 256); + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs) + else if (ticks <= 67107840L) + return (ticks / 1024); + //its really slow... hopefully we can just get by with super slow. + else + return 65535; +} + +byte getTimer1Resolution(unsigned long ticks) +{ + // these also represent frequency: 1000000 / ticks / 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 (ticks <= 65535L) + return 1; + // 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 (ticks <= 524280L) + return 2; + // 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 (ticks <= 4194240L) + return 3; + // 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 (ticks <= 16776960L) + return 4; + // 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 (ticks <= 67107840L) + return 5; + //its really slow... hopefully we can just get by with super slow. + else + return 5; +} + +inline void setTimer1Ticks(unsigned long ticks) +{ + // ticks is the delay between interrupts in 62.5 nanosecond 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. + + setTimer1Ceiling(getTimer1Ceiling(ticks)); + setTimer1Resolution(getTimer1Resolution(ticks)); +} + +inline void setTimer1Micros(unsigned long micros) +{ + //16 ticks in a microsecond. + unsigned long ticks = micros * 16; + + setTimer1Ticks(ticks); +} + +void setupTimer1Interrupt() +{ + //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. + setTimer1Resolution(5); + setTimer1Ceiling(65535); + disableTimer1Interrupt(); +} + +#endif diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Tools.pde b/tags/firmware/gen3/1.1/SanguinoMaster/Tools.pde new file mode 100755 index 00000000..1a36b4c5 --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Tools.pde @@ -0,0 +1,245 @@ +// Yep, this is actually -*- c++ -*- + +//initialize our tools +void init_tools() +{ + //do a scan of tools from address 0-255? + //with a 1 millisecond timeout, this takes ~0.256 seconds. + //we may also want to store which tools are available in eeprom? +/* +#ifdef SCAN_TOOLS_ON_STARTUP + for (int i=0; i<256; i++) + { + //are you out there? + if (ping_tool(i)) + { + init_tool(i); + } + } +#endif +*/ +} + +//ask a tool if its there. +bool ping_tool(byte i) +{ + slavePacket.init(); + + slavePacket.add_8(i); + slavePacket.add_8(SLAVE_CMD_VERSION); + slavePacket.add_16(FIRMWARE_VERSION); + return send_packet(); +} + +//initialize a tool to its default state. +void init_tool(byte i) +{ + slavePacket.init(); + + slavePacket.add_8(i); + slavePacket.add_8(SLAVE_CMD_INIT); + send_packet(); +} + +//select a tool as our current tool, and let it know. +void select_tool(byte tool) +{ + currentToolIndex = tool; + + slavePacket.init(); + + slavePacket.add_8(tool); + slavePacket.add_8(SLAVE_CMD_SELECT_TOOL); + send_packet(); + + wait_for_tool_ready_state(tool, 0, 0); +} + +void check_tool_ready_state() +{ + //did we timeout? + if (millis() >= toolTimeoutEnd) + { + commandMode = COMMAND_MODE_IDLE; + return; + } + + //is it time for the next ping? + if (millis() >= toolNextPing) + { + //is the tool ready? + if (is_tool_ready(currentToolIndex)) + { + commandMode = COMMAND_MODE_IDLE; + return; + } + + //ping every x millis + toolNextPing = millis() + 100; + } +} + +//ping the tool until it tells us its ready +void wait_for_tool_ready_state(byte tool, int delay_millis, int timeout_seconds) +{ + //setup some defaults + if (delay_millis == 0) + delay_millis = 100; + if (timeout_seconds == 0) + timeout_seconds = 60; + + //check for our end time. + unsigned long end = millis() + (timeout_seconds * 1000); + + //do it until we hear something, or time out. + while (1) + { + //did we time out yet? + if (millis() >= end) + return; + + //did we hear back from the tool? + if (is_tool_ready(tool)) + return; + + //try again... + delay(delay_millis); + } +} + +//is our tool ready for action? +bool is_tool_ready(byte tool) +{ + slavePacket.init(); + + slavePacket.add_8(tool); + slavePacket.add_8(SLAVE_CMD_IS_TOOL_READY); + + //did we get a response? + if (send_packet()) + { + //is it true? + if (slavePacket.get_8(1) == 1) + return true; + } + + //bail. + return false; +} + +void send_tool_query() +{ + //zero out our packet + slavePacket.init(); + + //load up our packet. + for (byte i=1; i<hostPacket.getLength(); i++) + slavePacket.add_8(hostPacket.get_8(i)); + + //send it and then get our response + send_packet(); + + //now load it up into the host. (skip the response code) + //TODO: check the response code + for (byte i=1; i<slavePacket.getLength(); i++) + hostPacket.add_8(slavePacket.get_8(i)); +} + +void send_tool_command() +{ + //zero out our packet + slavePacket.init(); + + //add in our tool id and command. + slavePacket.add_8(commandBuffer.remove_8()); + slavePacket.add_8(commandBuffer.remove_8()); + + //load up our packet. + byte len = commandBuffer.remove_8(); + for (byte i=0; i<len; i++) + slavePacket.add_8(commandBuffer.remove_8()); + + //send it and then get our response + send_packet(); +} + +boolean send_packet() +{ + //take it easy. no stomping on each other. + delayMicrosecondsInterruptible(50); + + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + + //take it easy. no stomping on each other. + delayMicrosecondsInterruptible(10); + + slavePacket.sendPacket(); + + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + + rs485_packet_count++; + + return read_tool_response(PACKET_TIMEOUT); +} + +bool read_tool_response(int timeout) +{ + //figure out our timeout stuff. + long start = millis(); + long end = start + timeout; + + //keep reading until we got it. + while (!slavePacket.isFinished()) + { + //read through our available data + if (Serial1.available() > 0) + { + //grab a byte and process it. + byte d = Serial1.read(); + slavePacket.process_byte(d); + + rs485_rx_count++; + +#ifdef ENABLE_COMMS_DEBUG + /* + Serial.print("IN:"); + Serial.print(d, HEX); + Serial.print("/"); + Serial.println(d, BIN); + */ +#endif + //keep processing while there's data. + start = millis(); + end = start + timeout; + + if (slavePacket.getResponseCode() == RC_CRC_MISMATCH) + { + slave_crc_errors++; + +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Slave CRC Mismatch"); +#endif + //retransmit? + } + } + + //not sure if we need this yet. + //our timeout guy. + if (millis() > end) + { + slave_timeouts++; + +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Slave Timeout"); +#endif + return false; + } + } + + return true; +} + +void set_tool_pause_state(bool paused) +{ + //TODO: pause/unpause tool. +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Utils.pde b/tags/firmware/gen3/1.1/SanguinoMaster/Utils.pde new file mode 100644 index 00000000..e09a86ff --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Utils.pde @@ -0,0 +1,22 @@ +void delayMicrosecondsInterruptible(unsigned int us) +{ + // 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; + + // busy wait + __asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles +"brne 1b" : + "=w" (us) : + "0" (us) // 2 cycles + ); +} diff --git a/tags/firmware/gen3/1.1/SanguinoMaster/Variables.h b/tags/firmware/gen3/1.1/SanguinoMaster/Variables.h new file mode 100644 index 00000000..6c1a2dce --- /dev/null +++ b/tags/firmware/gen3/1.1/SanguinoMaster/Variables.h @@ -0,0 +1,62 @@ +//this is the version of our host software +unsigned int host_version = 0; + +//these are our packet classes +SimplePacket hostPacket(serial_tx); +SimplePacket slavePacket(rs485_tx); + +//are we paused? +boolean is_machine_paused = false; +boolean is_machine_aborted = false; + +//init our variables +volatile long max_delta; + +volatile long x_counter; +volatile bool x_direction; + +volatile long y_counter; +volatile bool y_direction; + +volatile long z_counter; +volatile bool z_direction; + +//our position tracking variables +volatile LongPoint current_steps; +volatile LongPoint target_steps; +volatile LongPoint delta_steps; +volatile LongPoint range_steps; + +volatile bool is_point_queue_empty = true; + +//our point queue variables +uint8_t rawPointBuffer[POINT_QUEUE_SIZE * POINT_SIZE]; +CircularBuffer pointBuffer((POINT_QUEUE_SIZE * POINT_SIZE), rawPointBuffer); + +//buffer for our commands +uint8_t underlyingBuffer[COMMAND_BUFFER_SIZE]; +CircularBuffer commandBuffer(COMMAND_BUFFER_SIZE, underlyingBuffer); + +//how many queued commands have we processed? +//this will be used to keep track of our current progress. +unsigned long finishedCommands = 0; +unsigned long finishedPoints = 0; +byte commandMode = COMMAND_MODE_IDLE; + +unsigned int toolPingDelay = 100; +unsigned int toolTimeout = 120; +unsigned long toolNextPing = 0; +unsigned long toolTimeoutEnd = 0; +byte currentToolIndex = 0; + +#ifdef USE_SD_CARD +//SD CARD STUFF. +RepRapSDCard card; +File f; +#endif + +//our buffer of bytes. +#define BUFFSIZE 64 +char buffer[BUFFSIZE]; +byte bufferIndex = 0; +byte error = 0; diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Commands.h b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Commands.h new file mode 100644 index 00000000..d04d1c01 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Commands.h @@ -0,0 +1,48 @@ +// These are our query commands from the host +#define HOST_CMD_VERSION 0 +#define HOST_CMD_INIT 1 +#define HOST_CMD_GET_BUFFER_SIZE 2 +#define HOST_CMD_CLEAR_BUFFER 3 +#define HOST_CMD_GET_POSITION 4 +#define HOST_CMD_GET_RANGE 5 +#define HOST_CMD_SET_RANGE 6 +#define HOST_CMD_ABORT 7 +#define HOST_CMD_PAUSE 8 +#define HOST_CMD_PROBE 9 +#define HOST_CMD_TOOL_QUERY 10 + +// These are our bufferable commands from the host +#define HOST_CMD_QUEUE_POINT_INC 128 +#define HOST_CMD_QUEUE_POINT_ABS 129 +#define HOST_CMD_SET_POSITION 130 +#define HOST_CMD_FIND_AXES_MINIMUM 131 +#define HOST_CMD_FIND_AXES_MAXIMUM 132 +#define HOST_CMD_DELAY 133 +#define HOST_CMD_CHANGE_TOOL 134 +#define HOST_CMD_WAIT_FOR_TOOL 135 +#define HOST_CMD_TOOL_COMMAND 136 + +// These are our query commands from the host +#define SLAVE_CMD_VERSION 0 +#define SLAVE_CMD_INIT 1 +#define SLAVE_CMD_GET_TEMP 2 +#define SLAVE_CMD_SET_TEMP 3 +#define SLAVE_CMD_SET_MOTOR_1_PWM 4 +#define SLAVE_CMD_SET_MOTOR_2_PWM 5 +#define SLAVE_CMD_SET_MOTOR_1_RPM 6 +#define SLAVE_CMD_SET_MOTOR_2_RPM 7 +#define SLAVE_CMD_SET_MOTOR_1_DIR 8 +#define SLAVE_CMD_SET_MOTOR_2_DIR 9 +#define SLAVE_CMD_TOGGLE_MOTOR_1 10 +#define SLAVE_CMD_TOGGLE_MOTOR_2 11 +#define SLAVE_CMD_TOGGLE_FAN 12 +#define SLAVE_CMD_TOGGLE_VALVE 13 +#define SLAVE_CMD_SET_SERVO_1_POS 14 +#define SLAVE_CMD_SET_SERVO_2_POS 15 +#define SLAVE_CMD_FILAMENT_STATUS 16 +#define SLAVE_CMD_GET_MOTOR_1_PWM 17 +#define SLAVE_CMD_GET_MOTOR_2_PWM 18 +#define SLAVE_CMD_GET_MOTOR_1_RPM 19 +#define SLAVE_CMD_GET_MOTOR_2_RPM 20 +#define SLAVE_CMD_SELECT_TOOL 21 +#define SLAVE_CMD_IS_TOOL_READY 22
\ No newline at end of file diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Configuration.h b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Configuration.h new file mode 100644 index 00000000..14155938 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Configuration.h @@ -0,0 +1,196 @@ +/**************************************************************************************** + * Here's where you define the overall electronics setup for your machine. + ****************************************************************************************/ + +// This is -*- c++ -*-, no kidding +// define the parameters of our machine. +#define X_STEPS_PER_INCH 416.772354 +#define X_STEPS_PER_MM 11.7892 +#define X_MOTOR_STEPS 400 + +#define Y_STEPS_PER_INCH 416.772354 +#define Y_STEPS_PER_MM 11.0716 +#define Y_MOTOR_STEPS 400 + +#define Z_STEPS_PER_INCH 16256.0 +#define Z_STEPS_PER_MM 320.0 +#define Z_MOTOR_STEPS 400 + +//our maximum feedrates +#define FAST_XY_FEEDRATE 2500.0 +#define FAST_Z_FEEDRATE 1000.0 + +// Units in curve section +#define CURVE_SECTION_INCHES 0.019685 +#define CURVE_SECTION_MM 0.5 + +// Set to one if endstop outputs are inverting (ie: 1 means open, 0 means closed) +#define ENDSTOPS_INVERTING 1 + +// The *_ENABLE_PIN signals are active high as default. Define this +// to one if they should be active low instead (e.g. if you're using different +// stepper boards). +#define INVERT_ENABLE_PINS 1 + +// If you use this firmware on a cartesian platform where the +// stepper direction pins are inverted, set these defines to 1 +// for the axes which should be inverted. +// RepRap stepper boards are *not* inverting. +#define INVERT_X_DIR 0 +#define INVERT_Y_DIR 0 +#define INVERT_Z_DIR 0 + +// +// CHOOSE WHICH MOTHERBOARD YOU'RE USING: +// +#define REPRAP_MOTHERBOARD_VERSION_1_0 +//#define REPRAP_MOTHERBOARD_VERSION_1_1 + +// +// CHOOSE WHICH FAMILY OF STEPPER DRIVER YOU'RE USING: +// +//#define STEPPER_DRIVER_VERSION_1_X +#define STEPPER_DRIVER_VERSION_2_X + +// +// CHOOSE WHICH FAMILY OF OPTO ENDSTOP YOU'RE USING: +// +//#define OPTO_ENDSTOP_1_X +#define OPTO_ENDSTOP_2_X + +#define PACKET_TIMEOUT 500 +#define HOST_SERIAL_SPEED 19200 +#define SLAVE_SERIAL_SPEED 38400 + +//uncomment to enable debugging functions +#define ENABLE_DEBUG 1 +#define ENABLE_COMMS_DEBUG 1 + +//#define SCAN_TOOLS_ON_STARTUP + +/**************************************************************************************** + * Sanguino Pin Assignment + ****************************************************************************************/ + +//these are the pins for the v1.0 Motherboard. +#ifdef REPRAP_MOTHERBOARD_VERSION_1_0 + +//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 19 +#define Y_MIN_PIN 25 +#define Y_MAX_PIN 26 + +//z axis pins +#define Z_STEP_PIN 29 +#define Z_DIR_PIN 30 +#define Z_ENABLE_PIN 31 +#define Z_MIN_PIN 1 +#define Z_MAX_PIN 2 + +//our pin for debugging. +#define DEBUG_PIN 0 + +//our SD card pins +#define SD_CARD_SELECT 4 +#define SD_CARD_WRITE 28 +#define SD_CARD_DETECT 24 + + +//our RS485 pins +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 + + +#endif + +//these are the pins for the v1.1 Motherboard. +#ifdef REPRAP_MOTHERBOARD_VERSION_1_1 + +//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 + +//various SPI select pins +#define SPI_SELECT_1 1 +#define SPI_SELECT_2 3 +#define SPI_SELECT_3 14 + +//our SD card pins +#define SD_CARD_SELECT 4 +#define SD_CARD_WRITE 2 +#define SD_CARD_DETECT 3 + +//our RS485 pins +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 + +#endif + +/**************************************************************************************** + * Stepper Driver Behaviour Definition + ****************************************************************************************/ + +//do we want a step delay (ie: length of pulse in microseconds) comment out to disable. +#define STEP_DELAY 5 + +#ifdef STEPPER_DRIVER_VERSION_1_X +#define STEPPER_ENABLE 1 +#define STEPPER_DISABLE 0 +#endif + +#ifdef STEPPER_DRIVER_VERSION_2_X +#define STEPPER_ENABLE 0 +#define STEPPER_DISABLE 1 +#endif + +/**************************************************************************************** + * Opto Endstop Behaviour Definition + ****************************************************************************************/ + +#ifdef OPTO_ENDSTOP_1_X +#define SENSORS_INVERTING 0 +#endif + +#ifdef OPTO_ENDSTOP_2_X +#define SENSORS_INVERTING 1 +#endif + + +/**************************************************************************************** + * Various Buffer Size Declarations + ****************************************************************************************/ +//we store all queueable commands in one big giant buffer. +// Explicitly allocate memory at compile time for buffer. +#define COMMAND_BUFFER_SIZE 2048 +#define POINT_QUEUE_SIZE 32 +#define POINT_SIZE 9 +#define MAX_PACKET_LENGTH 32 diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/MasterGCodeInterpreter.pde b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/MasterGCodeInterpreter.pde new file mode 100644 index 00000000..9d337163 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/MasterGCodeInterpreter.pde @@ -0,0 +1,93 @@ +// Yep, this is actually -*- c++ -*- + +// Arduino G-code Interpreter +// 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) +#include <WProgram.h> +#include <SimplePacket.h> +#include "Commands.h" +#include "Configuration.h" +#include "RS485.h" +#include "Variables.h" + +//our command string +#define COMMAND_SIZE 128 +char word[COMMAND_SIZE]; +byte serial_count; +int no_data = 0; +long idle_time; +boolean comment = false; +boolean bytes_received = false; + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + digitalWrite(RX_ENABLE_PIN, LOW); //always listen. + + Serial.begin(HOST_SERIAL_SPEED); + Serial1.begin(SLAVE_SERIAL_SPEED); + + Serial.println("start"); + + //other initialization. + init_process_string(); + init_steppers(); + init_tool(0); +} + +void loop() +{ + char c; + + //read in characters if we got them. + if (Serial.available() > 0) + { + c = Serial.read(); + no_data = 0; + + //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) + { + word[serial_count] = c; + serial_count++; + } + if (c == ')') + comment = false; // End of comment - start listening again + } + + bytes_received = true; + } + //mark no data if nothing heard for 100 milliseconds + else + { + if ((millis() - idle_time) >= 100) + { + no_data++; + idle_time = millis(); + } + } + + //if theres a pause or we got a real command, do it + if (bytes_received && (c == '\n' || no_data )) + { + //process our command! + process_string(word, serial_count); + + //clear command. + init_process_string(); + } + + //no data? turn off steppers + if (no_data > 10 ) + disable_steppers(); +} diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/RS485.h b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/RS485.h new file mode 100644 index 00000000..14c8a778 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/RS485.h @@ -0,0 +1,41 @@ +unsigned long rs485_tx_count = 0; +unsigned long rs485_rx_count = 0; +unsigned long rs485_packet_count = 0; +unsigned long rs485_loopback_fails = 0; +unsigned long slave_crc_errors = 0; +unsigned long slave_timeouts = 0; + +unsigned long serial_tx_count = 0; +unsigned long serial_rx_count = 0; +unsigned long serial_packet_count = 0; + +void rs485_tx(byte b) +{ + rs485_tx_count++; + + Serial1.print(b, BYTE); + + //read for our own byte. + long now = millis(); + long end = now + 10; + int tmp = Serial1.read(); + while (tmp != b) + { + tmp = Serial1.read(); + if (millis() > end) + { + rs485_loopback_fails++; +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Loopback Fail"); +#endif + break; + } + } +} + +void serial_tx(byte b) +{ + serial_tx_count++; + + Serial.print(b, BYTE); +} diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/ThermistorTable.h b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/ThermistorTable.h new file mode 100644 index 00000000..6a071e28 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/ThermistorTable.h @@ -0,0 +1,37 @@ +#ifndef THERMISTORTABLE_H_ +#define THERMISTORTABLE_H_ + +// Thermistor lookup table for RepRap Temperature Sensor Boards (http://make.rrrf.org/ts) +// 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 +#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 diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Variables.h b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Variables.h new file mode 100644 index 00000000..9295aab2 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/Variables.h @@ -0,0 +1,6 @@ +//this is the version of our host software +unsigned int host_version = 0; + +//these are our packet classes +SimplePacket slavePacket(rs485_tx); + diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/_init.pde.dist b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/_init.pde.dist new file mode 100644 index 00000000..b1464dbc --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/_init.pde.dist @@ -0,0 +1,78 @@ +// This is -*- c++ -*-, no kidding +// define the parameters of our machine. +#define X_STEPS_PER_INCH 416.772354 +#define X_STEPS_PER_MM 16.4083604 +#define X_MOTOR_STEPS 400 + +#define Y_STEPS_PER_INCH 416.772354 +#define Y_STEPS_PER_MM 16.4083604 +#define Y_MOTOR_STEPS 400 + +#define Z_STEPS_PER_INCH 16256.0 +#define Z_STEPS_PER_MM 640.0 +#define Z_MOTOR_STEPS 400 + +//our maximum feedrates +#define FAST_XY_FEEDRATE 1000.0 +#define FAST_Z_FEEDRATE 50.0 + +// Units in curve section +#define CURVE_SECTION_INCHES 0.019685 +#define CURVE_SECTION_MM 0.5 + +// Set to one if endstop outputs are inverting (ie: 1 means open, 0 means closed) +// RepRap opto endstops are *not* inverting. +#define ENDSTOPS_INVERTING 0 +// Optionally disable max endstops to save pins or wiring +#define ENDSTOPS_MIN_ENABLED 1 +#define ENDSTOPS_MAX_ENABLED 0 + +// How many temperature samples to take. each sample takes about 100 usecs. +#define TEMPERATURE_SAMPLES 5 + +// The *_ENABLE_PIN signals are active high as default. Define this +// to one if they should be active low instead (e.g. if you're using different +// stepper boards). +// RepRap stepper boards are *not* inverting. +#define INVERT_ENABLE_PINS 0 + +// If you use this firmware on a cartesian platform where the +// stepper direction pins are inverted, set these defines to 1 +// for the axes which should be inverted. +// RepRap stepper boards are *not* inverting. +#define INVERT_X_DIR 0 +#define INVERT_Y_DIR 0 +#define INVERT_Z_DIR 0 + +/**************************************************************************************** +* digital i/o pin assignment +* +* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5 +****************************************************************************************/ + +//cartesian bot pins +#define X_STEP_PIN 2 +#define X_DIR_PIN 3 +#define X_MIN_PIN 4 +#define X_MAX_PIN 9 +#define X_ENABLE_PIN 15 + +#define Y_STEP_PIN 10 +#define Y_DIR_PIN 7 +#define Y_MIN_PIN 8 +#define Y_MAX_PIN 13 +#define Y_ENABLE_PIN 15 + +#define Z_STEP_PIN 19 +#define Z_DIR_PIN 18 +#define Z_MIN_PIN 17 +#define Z_MAX_PIN 16 +#define Z_ENABLE_PIN 15 + +//extruder pins +#define EXTRUDER_MOTOR_SPEED_PIN 11 +#define EXTRUDER_MOTOR_DIR_PIN 12 +#define EXTRUDER_HEATER_PIN 6 +#define EXTRUDER_FAN_PIN 5 +#define EXTRUDER_THERMISTOR_PIN 0 //NB! analog pin, -1 disables thermistor readings +#define EXTRUDER_THERMOCOUPLE_PIN -1 //NB! analog pin, -1 disables thermocouple readings diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/extruder.pde b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/extruder.pde new file mode 100644 index 00000000..d46262ea --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/extruder.pde @@ -0,0 +1,409 @@ +// Yep, this is actually -*- c++ -*- + + +//initialize a tool to its default state. +void init_tool(byte i) +{ + slavePacket.init(); + + slavePacket.add_8(i); + slavePacket.add_8(SLAVE_CMD_INIT); + send_packet(); +} + +//select a tool as our current tool, and let it know. +void select_tool(byte tool) +{ + slavePacket.init(); + + slavePacket.add_8(tool); + slavePacket.add_8(SLAVE_CMD_SELECT_TOOL); + send_packet(); +} + +//ping the tool until it tells us its ready +void wait_for_tool_ready_state(byte tool, int delay_millis, int timeout_seconds) +{ + //setup some defaults + if (delay_millis == 0) + delay_millis = 100; + if (timeout_seconds == 0) + timeout_seconds = 60; + + //check for our end time. + unsigned long end = millis() + (timeout_seconds * 1000); + + //do it until we hear something, or time out. + while (1) + { + //did we time out yet? + if (millis() >= end) + return; + + //did we hear back from the tool? + if (is_tool_ready(tool)) + return; + + //try again... + delay(delay_millis); + } +} + +//is our tool ready for action? +bool is_tool_ready(byte tool) +{ + slavePacket.init(); + + slavePacket.add_8(tool); + slavePacket.add_8(SLAVE_CMD_IS_TOOL_READY); + + //did we get a response? + if (send_packet()) + { + //is it true? + if (slavePacket.get_8(1) == 1) + return true; + } + + //bail. + return false; +} + +boolean send_packet() +{ + //take it easy. no stomping on each other. + delayMicrosecondsInterruptible(50); + + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + + //take it easy. no stomping on each other. + delayMicrosecondsInterruptible(10); + + slavePacket.sendPacket(); + + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + + rs485_packet_count++; + + return read_tool_response(PACKET_TIMEOUT); +} + +bool read_tool_response(int timeout) +{ + //figure out our timeout stuff. + long start = millis(); + long end = start + timeout; + + //keep reading until we got it. + while (!slavePacket.isFinished()) + { + //read through our available data + if (Serial1.available() > 0) + { + //grab a byte and process it. + byte d = Serial1.read(); + slavePacket.process_byte(d); + + rs485_rx_count++; + +#ifdef ENABLE_COMMS_DEBUG + /* + Serial.print("IN:"); + Serial.print(d, HEX); + Serial.print("/"); + Serial.println(d, BIN); + */ +#endif + //keep processing while there's data. + start = millis(); + end = start + timeout; + + if (slavePacket.getResponseCode() == RC_CRC_MISMATCH) + { + slave_crc_errors++; + +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Slave CRC Mismatch"); +#endif + //retransmit? + } + } + + //not sure if we need this yet. + //our timeout guy. + if (millis() > end) + { + slave_timeouts++; + +#ifdef ENABLE_COMMS_DEBUG + Serial.println("Slave Timeout"); +#endif + return false; + } + } + + return true; +} + +void set_tool_pause_state(bool paused) +{ + //TODO: pause/unpause tool. +} + +#ifdef ENABLE_DEBUG + +void exercise_heater() +{ + set_tool_temp(1, 100); + while (1) + { + get_tool_temp(1); + delay(500); + } +} + +void exercise_motors() +{ + boolean dir = true; + + Serial.println("forward"); + Serial.println("up"); + for (int i=0; i<256; i++) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + Serial.println("down"); + for (int i=255; i>=0; i--) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + dir = false; + + Serial.println("forward"); + Serial.println("up"); + for (int i=0; i<256; i++) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } + + Serial.println("down"); + for (int i=255; i>=0; i--) + { + set_motor1_pwm(1, i); + toggle_motor1(1, dir, 1); + set_motor2_pwm(1, i); + toggle_motor2(1, dir, 1); + Serial.println(i, DEC); + } +} + +void print_stats() +{ + Serial.println("Stats:"); + Serial.print("Slave TX Count:"); + Serial.println(rs485_tx_count, DEC); + Serial.print("Slave RX Count:"); + Serial.println(rs485_rx_count, DEC); + Serial.print("Slave Packet Count: "); + Serial.println(rs485_packet_count, DEC); + Serial.print("Slave CRC Errors: "); + Serial.println(slave_crc_errors, DEC); + Serial.print("Slave timeouts: "); + Serial.println(slave_timeouts, DEC); +} + +void print_tool(byte id) +{ + Serial.print("tool #"); + Serial.print(id, DEC); + Serial.print(" "); +} + +int get_tool_temp(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_TEMP); + send_packet(); + + int temp = slavePacket.get_16(1); + return temp; +} + +void set_tool_temp(byte id, unsigned int temp) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_TEMP); + slavePacket.add_16(temp); + send_packet(); + + print_tool(id); + Serial.print("set temp to: "); + Serial.println(temp, DEC); +} + +void set_motor1_pwm(byte id, byte pwm) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_MOTOR_1_PWM); + slavePacket.add_8(pwm); + send_packet(); + + //print_tool(id); + //Serial.print("set motor1 pwm to: "); + //Serial.println(pwm, DEC); +} + +void set_motor2_pwm(byte id, byte pwm) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_MOTOR_2_PWM); + slavePacket.add_8(pwm); + send_packet(); + + //print_tool(id); + //Serial.print("set motor2 pwm to: "); + //Serial.println(pwm, DEC); +} + + +void set_motor1_rpm(byte id, int rpm) +{ + +} + +void toggle_motor1(byte id, boolean dir, boolean enable) +{ + byte flags = 0; + + if (enable) + flags += 1; + + if (dir) + flags += 2; + + Serial.println(flags, BIN); + + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_MOTOR_1); + slavePacket.add_8(flags); + send_packet(); +} + +void toggle_motor2(byte id, boolean dir, boolean enable) +{ + byte flags = 0; + + if (enable) + flags += 1; + + if (dir) + flags += 2; + + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_MOTOR_2); + slavePacket.add_8(flags); + send_packet(); +} + +void toggle_fan(byte id, boolean enable) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_FAN); + slavePacket.add_8(enable); + send_packet(); +} + +void toggle_valve(byte id, boolean open) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_TOGGLE_VALVE); + slavePacket.add_8(open); + send_packet(); +} + +void get_motor1_pwm(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_MOTOR_1_PWM); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("m1 pwm: "); + Serial.println(temp, DEC); +} + +void get_motor2_pwm(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_GET_MOTOR_2_PWM); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("m2 pwm: "); + Serial.println(temp, DEC); +} + +void set_servo1_position(byte id, byte pos) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_SERVO_1_POS); + slavePacket.add_8(pos); + send_packet(); +} + +void set_servo2_position(byte id, byte pos) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_SET_SERVO_2_POS); + slavePacket.add_8(pos); + send_packet(); +} + +void get_motor1_rpm(byte id) +{ + +} + +void get_filament_status(byte id) +{ + slavePacket.init(); + slavePacket.add_8(id); + slavePacket.add_8(SLAVE_CMD_FILAMENT_STATUS); + send_packet(); + + byte temp = slavePacket.get_8(1); + print_tool(id); + Serial.print("filament: "); + Serial.println(temp, DEC); +} + +#endif + diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/process_string.pde b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/process_string.pde new file mode 100644 index 00000000..fb7ad2d9 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/process_string.pde @@ -0,0 +1,521 @@ +// Yep, this is actually -*- c++ -*- + +// our point structure to make things nice. +struct LongPoint +{ + long x; + long y; + long z; +}; + +struct FloatPoint +{ + float x; + float y; + float z; +}; + +/* gcode line parse results */ +struct GcodeParser +{ + unsigned int seen; + int G; + int M; + float P; + float X; + float Y; + float Z; + float I; + float J; + float F; + float S; + float R; + float Q; +}; + +FloatPoint current_units; +FloatPoint target_units; +FloatPoint delta_units; + +FloatPoint current_steps; +FloatPoint target_steps; +FloatPoint delta_steps; + +boolean abs_mode = false; //0 = incremental; 1 = absolute + +//default to mm for units +float x_units = X_STEPS_PER_MM; +float y_units = Y_STEPS_PER_MM; +float z_units = Z_STEPS_PER_MM; +float curve_section = CURVE_SECTION_MM; + +//our direction vars +byte x_direction = 1; +byte y_direction = 1; +byte z_direction = 1; + +int scan_int(char *str, int *valp); +int scan_float(char *str, float *valp); + +//init our string processing +void init_process_string() +{ + //init our command + for (byte i=0; i<COMMAND_SIZE; i++) + word[i] = 0; + serial_count = 0; + bytes_received = false; + + idle_time = millis(); +} + +//our feedrate variables. +float feedrate = 0.0; +long feedrate_micros = 0; + +/* 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; + +/* 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 TYPE_INT 1 +#define TYPE_FLOAT 2 + +/* macros to save typing and bugs in the parser function */ +#define PARSE_INT(ch, instr, str, str_size, len, val, seen, flag) \ + case ch: \ + len = scan_int(str, &val, &seen, flag); \ + break; + +#define PARSE_FLOAT(ch, instr, str, str_size, len, val, seen, flag) \ + case ch: \ + len = scan_float(str, &val, &seen, flag); \ + break; + +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, &instruction[ind+1], size-ind, len, gc->G, gc->seen, GCODE_G); + PARSE_INT('M', instruction, &instruction[ind+1], size-ind, len, gc->M, gc->seen, GCODE_M); + PARSE_FLOAT('S', instruction, &instruction[ind+1], size-ind, len, gc->S, gc->seen, GCODE_S); + PARSE_FLOAT('P', instruction, &instruction[ind+1], size-ind, len, gc->P, gc->seen, GCODE_P); + PARSE_FLOAT('X', instruction, &instruction[ind+1], size-ind, len, gc->X, gc->seen, GCODE_X); + PARSE_FLOAT('Y', instruction, &instruction[ind+1], size-ind, len, gc->Y, gc->seen, GCODE_Y); + PARSE_FLOAT('Z', instruction, &instruction[ind+1], size-ind, len, gc->Z, gc->seen, GCODE_Z); + PARSE_FLOAT('I', instruction, &instruction[ind+1], size-ind, len, gc->I, gc->seen, GCODE_I); + PARSE_FLOAT('J', instruction, &instruction[ind+1], size-ind, len, gc->J, gc->seen, GCODE_J); + PARSE_FLOAT('F', instruction, &instruction[ind+1], size-ind, len, gc->F, gc->seen, GCODE_F); + PARSE_FLOAT('R', instruction, &instruction[ind+1], size-ind, len, gc->R, gc->seen, GCODE_R); + PARSE_FLOAT('Q', instruction, &instruction[ind+1], size-ind, len, gc->Q, gc->seen, GCODE_Q); + break; + } + } +} + +GcodeParser gc; /* string parse result */ + +//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] == '/') + { + Serial.println("ok"); + return; + } + + //init baby! + FloatPoint fp; + fp.x = 0.0; + fp.y = 0.0; + fp.z = 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))) && + ((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 = current_units; + 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; + } + 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; + } + + // Get feedrate if supplied + if ( gc.seen & GCODE_F ) + feedrate = gc.F; + + //do something! + switch (gc.G) + { + //Rapid Positioning + //Linear Interpolation + //these are basically the same thing. + case 0: + case 1: + //set our target. + set_target(fp.x, fp.y, fp.z); + + //split up xy and z moves + if (delta_steps.z && (delta_steps.x || delta_steps.y)) + { + set_target(current_units.x, current_units.y, fp.z); + figure_feedrate(gc.G); + dda_move(feedrate_micros); + } + + //do the last move + set_target(fp.x, fp.y, fp.z); + figure_feedrate(gc.G); + dda_move(feedrate_micros); + break; + + case 4: //Dwell + delay((int)(gc.P * 1000)); + break; + + //Inches for Units + case 20: + x_units = X_STEPS_PER_INCH; + y_units = Y_STEPS_PER_INCH; + z_units = Z_STEPS_PER_INCH; + curve_section = CURVE_SECTION_INCHES; + + calculate_deltas(); + break; + + //mm for Units + case 21: + x_units = X_STEPS_PER_MM; + y_units = Y_STEPS_PER_MM; + z_units = Z_STEPS_PER_MM; + curve_section = CURVE_SECTION_MM; + + calculate_deltas(); + break; + + //go home. + case 28: + set_target(0.0, 0.0, 0.0); + dda_move(getMaxSpeed()); + break; + + //go home via an intermediate point. + case 30: + //set our target. + set_target(fp.x, fp.y, fp.z); + + //go there. + dda_move(getMaxSpeed()); + + //go home. + set_target(0.0, 0.0, 0.0); + dda_move(getMaxSpeed()); + break; +/* + // Drilling canned cycles + case 81: // Without dwell + case 82: // With dwell + case 83: // Peck drilling + { + float retract = gc.R; + + if (!abs_mode) + retract += current_units.z; + + // Retract to R position if Z is currently below this + if (current_units.z < retract) + { + set_target(current_units.x, current_units.y, retract); + dda_move(getMaxSpeed()); + } + + // Move to start XY + set_target(fp.x, fp.y, current_units.z); + dda_move(getMaxSpeed()); + + // Do the actual drilling + float target_z = retract; + float delta_z; + + // For G83 move in increments specified by Q code, otherwise do in one pass + if (gc.G == 83) + delta_z = gc.Q; + else + delta_z = retract - fp.z; + + do { + // Move rapidly to bottom of hole drilled so far (target Z if starting hole) + set_target(fp.x, fp.y, target_z); + dda_move(getMaxSpeed()); + + // Move with controlled feed rate by delta z (or to bottom of hole if less) + target_z -= delta_z; + if (target_z < fp.z) + target_z = fp.z; + set_target(fp.x, fp.y, target_z); + if (feedrate > 0) + feedrate_micros = calculate_feedrate_delay(feedrate); + else + feedrate_micros = getMaxSpeed(); + dda_move(feedrate_micros); + + // Dwell if doing a G82 + if (gc.G == 82) + delay((int)(gc.P * 1000)); + + // Retract + set_target(fp.x, fp.y, retract); + dda_move(getMaxSpeed()); + } while (target_z > fp.z); + } + break; +*/ + + case 90: //Absolute Positioning + abs_mode = true; + break; + + + case 91: //Incremental Positioning + abs_mode = false; + break; + + + case 92: //Set as home + set_position(0.0, 0.0, 0.0); + break; + + /* + //Inverse Time Feed Mode + case 93: + + break; //TODO: add this + + //Feed per Minute Mode + case 94: + + break; //TODO: add this + */ + + default: + Serial.print("huh? G"); + Serial.println(gc.G, DEC); + } + } + + //find us an m code. + if (gc.seen & GCODE_M) + { + int temp = 0; + switch (gc.M) + { + //TODO: this is a bug because search_string returns 0. gotta fix that. + case 0: + true; + break; + /* + case 0: + //todo: stop program + break; + + case 1: + //todo: optional stop + break; + + case 2: + //todo: program end + break; + */ + //turn extruder on, forward + case 101: + toggle_motor1(0, 1, 1); + break; + + //turn extruder on, reverse + case 102: + toggle_motor1(0, 0, 1); + break; + + //turn extruder off + case 103: + toggle_motor1(0, 0, 0); + break; + + //custom code for temperature control + case 104: + set_tool_temp(0, (int)gc.S); + + temp = 0; + while (temp < (int)gc.S) + { + temp = get_tool_temp(0); + Serial.print("T:"); + Serial.println(temp, DEC); + delay(500); + } + + break; + + //custom code for temperature reading + case 105: + Serial.print("T:"); + Serial.println(get_tool_temp(0)); + break; + + //turn fan on + case 106: + toggle_fan(0, 1); + break; + + //turn fan off + case 107: + toggle_fan(0, 0); + break; + + //set max extruder speed, 0-255 PWM + case 108: + if (gc.seen & GCODE_S) + set_motor1_pwm(0, (int)gc.S); + break; + + default: + Serial.print("Huh? M"); + Serial.println(gc.M, DEC); + } + } + + //tell our host we're done + Serial.println("ok"); +} + +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 */ +} + +void figure_feedrate(byte code) +{ + //do we have a set speed? + if (gc.seen & GCODE_G) + { + //adjust if we have a specific feedrate. + if (code == 1) + { + //how fast do we move? + feedrate = gc.F; + if (feedrate > 0) + feedrate_micros = calculate_feedrate_delay(feedrate); + //nope, no feedrate + else + feedrate_micros = getMaxSpeed(); + } + //use our max for normal moves. + else + feedrate_micros = getMaxSpeed(); + } + //nope, just coordinates! + else + { + //do we have a feedrate yet? + if (feedrate > 0) + feedrate_micros = calculate_feedrate_delay(feedrate); + //nope, no feedrate + else + feedrate_micros = getMaxSpeed(); + } + + feedrate_micros = max(feedrate_micros, getMaxSpeed()); +} diff --git a/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/stepper_control.pde b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/stepper_control.pde new file mode 100644 index 00000000..a0422870 --- /dev/null +++ b/tags/firmware/gen3/1.1/gcode/MasterGCodeInterpreter/stepper_control.pde @@ -0,0 +1,390 @@ +// Yep, this is actually -*- c++ -*- + +//init our variables +long max_delta; +long x_counter; +long y_counter; +long z_counter; +bool x_can_step; +bool y_can_step; +bool z_can_step; +int milli_delay; + +#if INVERT_ENABLE_PINS == 1 +#define ENABLE_ON LOW +#else +#define ENABLE_ON HIGH +#endif + +void init_steppers() +{ + //turn them off to start. + disable_steppers(); + + //init our points. + current_units.x = 0.0; + current_units.y = 0.0; + current_units.z = 0.0; + target_units.x = 0.0; + target_units.y = 0.0; + target_units.z = 0.0; + + pinMode(X_STEP_PIN, OUTPUT); + pinMode(X_DIR_PIN, OUTPUT); + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(Y_STEP_PIN, OUTPUT); + pinMode(Y_DIR_PIN, OUTPUT); + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Z_STEP_PIN, OUTPUT); + pinMode(Z_DIR_PIN, OUTPUT); + pinMode(Z_ENABLE_PIN, OUTPUT); + +#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 + + //figure our stuff. + calculate_deltas(); +} + +void dda_move(long micro_delay) +{ + //turn on steppers to start moving =) + enable_steppers(); + + //figure out our deltas + max_delta = max(delta_steps.x, delta_steps.y); + max_delta = max(delta_steps.z, max_delta); + + //init stuff. + long x_counter = -max_delta/2; + long y_counter = -max_delta/2; + long z_counter = -max_delta/2; + + //our step flags + bool x_can_step = 0; + bool y_can_step = 0; + bool z_can_step = 0; + + //how long do we delay for? + if (micro_delay >= 16383) + milli_delay = micro_delay / 1000; + else + milli_delay = 0; + + //do our DDA line! + 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); + + if (x_can_step) + { + x_counter += delta_steps.x; + + if (x_counter > 0) + { + do_step(X_STEP_PIN); + x_counter -= max_delta; + + if (x_direction) + current_steps.x++; + else + current_steps.x--; + } + } + + if (y_can_step) + { + y_counter += delta_steps.y; + + if (y_counter > 0) + { + do_step(Y_STEP_PIN); + y_counter -= max_delta; + + if (y_direction) + current_steps.y++; + else + current_steps.y--; + } + } + + if (z_can_step) + { + z_counter += delta_steps.z; + + if (z_counter > 0) + { + do_step(Z_STEP_PIN); + z_counter -= max_delta; + + if (z_direction) + current_steps.z++; + else + current_steps.z--; + } + } + + //wait for next step. + if (milli_delay > 0) + delay(milli_delay); + else + delayMicrosecondsInterruptible(micro_delay); + } + while (x_can_step || y_can_step || z_can_step); + + //set our points to be the same + current_units.x = target_units.x; + current_units.y = target_units.y; + current_units.z = target_units.z; + calculate_deltas(); +} + +bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction) +{ + //stop us if we're on target + if (target == current) + return false; +#if ENDSTOPS_MIN_ENABLED == 1 + //stop us if we're at home and still going + else if (read_switch(min_pin) && !direction) + return false; +#endif +#if ENDSTOPS_MAX_ENABLED == 1 + //stop us if we're at max and still going + else if (read_switch(max_pin) && direction) + return false; +#endif + + //default to being able to step + return true; +} + +void do_step(byte step_pin) +{ + digitalWrite(step_pin, HIGH); + delayMicroseconds(5); + digitalWrite(step_pin, LOW); +} + +bool 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 +} + +long to_steps(float steps_per_unit, float units) +{ + return steps_per_unit * units; +} + +void set_target(float x, float y, float z) +{ + target_units.x = x; + target_units.y = y; + target_units.z = z; + + calculate_deltas(); +} + +void set_position(float x, float y, float z) +{ + current_units.x = x; + current_units.y = y; + current_units.z = z; + + calculate_deltas(); +} + +void calculate_deltas() +{ + //figure our deltas. + delta_units.x = abs(target_units.x - current_units.x); + delta_units.y = abs(target_units.y - current_units.y); + delta_units.z = abs(target_units.z - current_units.z); + + //set our steps current, target, and delta + current_steps.x = to_steps(x_units, current_units.x); + current_steps.y = to_steps(y_units, current_units.y); + current_steps.z = to_steps(z_units, current_units.z); + + target_steps.x = to_steps(x_units, target_units.x); + target_steps.y = to_steps(y_units, target_units.y); + target_steps.z = to_steps(z_units, target_units.z); + + delta_steps.x = abs(target_steps.x - current_steps.x); + delta_steps.y = abs(target_steps.y - current_steps.y); + delta_steps.z = abs(target_steps.z - current_steps.z); + + //what is our direction + x_direction = (target_units.x >= current_units.x); + y_direction = (target_units.y >= current_units.y); + z_direction = (target_units.z >= current_units.z); + + //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 +} + + +long calculate_feedrate_delay(float feedrate) +{ + //how long is our line length? + float distance = sqrt(delta_units.x*delta_units.x + + delta_units.y*delta_units.y + + delta_units.z*delta_units.z); + long master_steps = 0; + + //find the dominant axis. + if (delta_steps.x > delta_steps.y) + { + if (delta_steps.z > delta_steps.x) + master_steps = delta_steps.z; + else + master_steps = delta_steps.x; + } + else + { + if (delta_steps.z > delta_steps.y) + master_steps = delta_steps.z; + else + master_steps = delta_steps.y; + } + + //calculate delay between steps in microseconds. this is sort of tricky, but not too bad. + //the formula has been condensed to save space. here it is in english: + // (feedrate is in mm/minute) + // distance / feedrate * 60000000.0 = move duration in microseconds + // move duration / master_steps = time between steps for master axis. + + return ((distance * 60000000.0) / feedrate) / master_steps; +} + +long getMaxSpeed() +{ + if (delta_steps.z > 0) + return calculate_feedrate_delay(FAST_Z_FEEDRATE); + else + return calculate_feedrate_delay(FAST_XY_FEEDRATE); +} + +void enable_steppers() +{ + // Enable steppers only for axes which are moving + // taking account of the fact that some or all axes + // may share an enable line (check using macros at + // compile time to avoid needless code) + if ( target_units.x == current_units.x + #if X_ENABLE_PIN == Y_ENABLE_PIN + && target_units.y == current_units.y + #endif + #if X_ENABLE_PIN == Z_ENABLE_PIN + && target_units.z == current_units.z + #endif + ) + digitalWrite(X_ENABLE_PIN, !ENABLE_ON); + else + digitalWrite(X_ENABLE_PIN, ENABLE_ON); + if ( target_units.y == current_units.y + #if Y_ENABLE_PIN == X_ENABLE_PIN + && target_units.x == current_units.x + #endif + #if Y_ENABLE_PIN == Z_ENABLE_PIN + && target_units.z == current_units.z + #endif + ) + digitalWrite(Y_ENABLE_PIN, !ENABLE_ON); + else + digitalWrite(Y_ENABLE_PIN, ENABLE_ON); + if ( target_units.z == current_units.z + #if Z_ENABLE_PIN == X_ENABLE_PIN + && target_units.x == current_units.x + #endif + #if Z_ENABLE_PIN == Y_ENABLE_PIN + && target_units.y == current_units.y + #endif + ) + digitalWrite(Z_ENABLE_PIN, !ENABLE_ON); + else + digitalWrite(Z_ENABLE_PIN, ENABLE_ON); +} + +void disable_steppers() +{ + //disable our steppers + digitalWrite(X_ENABLE_PIN, !ENABLE_ON); + digitalWrite(Y_ENABLE_PIN, !ENABLE_ON); + digitalWrite(Z_ENABLE_PIN, !ENABLE_ON); +} + +void 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/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.cpp b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.cpp new file mode 100755 index 00000000..704da49d --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.cpp @@ -0,0 +1,147 @@ +#include <avr/io.h> +#include "RepRapSDCard.h" +#include "fat16.h" +#include "sd_raw.h" +#include "partition.h" +#include <string.h> + + +#if !USE_DYNAMIC_MEMORY + struct partition_struct partition_handles[PARTITION_COUNT]; +#endif + +RepRapSDCard::RepRapSDCard(void) +{ + //do nothing. +} + +uint8_t RepRapSDCard::init_card(void) +{ + return sd_raw_init(); +} + +uint8_t RepRapSDCard::isLocked(void) +{ + return sd_raw_locked() == 0x00; +} + +uint8_t RepRapSDCard::isAvailable(void) +{ + return sd_raw_available(); +} + + +uint8_t RepRapSDCard::open_partition(void) +{ + /* open first partition */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, + sd_raw_write, + sd_raw_write_interval, + 0); + + if(!partition) + { + /* If the partition did not open, assume the storage device + * is a "superfloppy", i.e. has no MBR. + */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, + sd_raw_write, + sd_raw_write_interval, + -1); + } + + if(!partition) + return 0; + return 1; +} + +uint8_t RepRapSDCard::open_filesys(void) +{ + /* open file system */ + fs = fat16_open(partition); + if(!fs) + return 0; + + return 1; +} + +uint8_t RepRapSDCard::open_dir(char *path) +{ + // Open root directory + struct fat16_dir_entry_struct rootdirectory; + + fat16_get_dir_entry_of_root(fs, &rootdirectory); + dd = fat16_open_dir(fs, &rootdirectory); + if(!dd) + return 0; + + return 1; +} + + +uint8_t find_file_in_dir(struct fat16_fs_struct* fs, struct fat16_dir_struct* dd, const char* name, struct fat16_dir_entry_struct* dir_entry) +{ + while(fat16_read_dir(dd, dir_entry)) + { + if(strcmp(dir_entry->long_name, name) == 0) + { + fat16_reset_dir(dd); + return 1; + } + } + + return 0; +} + +struct fat16_file_struct* open_file_in_dir(struct fat16_fs_struct* fs, struct fat16_dir_struct* dd, const char* name) +{ + struct fat16_dir_entry_struct file_entry; + + if(!find_file_in_dir(fs, dd, name, &file_entry)) + { + //Serial.println("File not found"); + return 0; + } + + return fat16_open_file(fs, &file_entry); +} + + +File RepRapSDCard::open_file(char *name) +{ + return open_file_in_dir(fs, dd, name); +} + + +uint8_t RepRapSDCard::create_file(char *name) +{ + struct fat16_dir_entry_struct file_entry; + return fat16_create_file(dd, name, &file_entry); +} + +uint8_t RepRapSDCard::reset_file(File f) +{ + return fat16_seek_file(f, 0, FAT16_SEEK_SET); +} + +uint8_t RepRapSDCard::seek_file(File f, int32_t *offset, uint8_t whence) +{ + return fat16_seek_file(f, offset, whence); +} + +uint16_t RepRapSDCard::read_file(File f, uint8_t* buffer, uint16_t buffer_len) +{ + return fat16_read_file(f, buffer, buffer_len); +} + +uint8_t RepRapSDCard::write_file(File f, uint8_t *buff, uint8_t siz) +{ + return fat16_write_file(f, buff, siz); +} + +void RepRapSDCard::close_file(File f) +{ + fat16_close_file(f); +} diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.h new file mode 100755 index 00000000..e1d15b8a --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/RepRapSDCard.h @@ -0,0 +1,41 @@ +#ifndef _REPRAPSDCARD_H_ +#define _REPRAPSDCARD_H_ + +#include "sd_raw.h" +#include "sd_raw_config.h" +#include "sd-reader_config.h" +#include "partition.h" +#include "partition_config.h" +#include "fat16.h" + +typedef struct fat16_file_struct * File; + +class RepRapSDCard +{ + //private? + struct partition_struct *partition; + struct fat16_fs_struct* fs; + struct fat16_dir_struct* dd; + struct fat16_dir_entry_struct file_entry; + + public: + RepRapSDCard(void); + uint8_t isAvailable(void); + uint8_t isLocked(void); + uint8_t init_card(void); + uint8_t open_partition(void); + uint8_t open_filesys(void); + uint8_t open_dir(char *path); + uint8_t close_dir(void); + char *get_next_name_in_dir(void); + void reset_dir(void); + File open_file(char *name); + void close_file(File f); + uint8_t create_file(char *name); + uint8_t seek_file(File fd, int32_t *offset, uint8_t whence); + uint8_t reset_file(File f); + uint16_t read_file(File f, uint8_t* buffer, uint16_t buffer_len); + uint8_t write_file(File f, uint8_t *b, uint8_t num); +}; + +#endif diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.cpp b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.cpp new file mode 100755 index 00000000..2d9a64f1 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.cpp @@ -0,0 +1,1727 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#include "RepRapSDCard.h" +#include "partition.h" +#include "fat16.h" +#include "fat16_config.h" +#include "sd-reader_config.h" + +#include <string.h> + +#if USE_DYNAMIC_MEMORY + #include <stdlib.h> +#endif + +static uint8_t fat16_read_header(struct fat16_fs_struct* fs); +static uint16_t fat16_get_next_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint16_t fat16_append_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num, uint16_t count); +static uint8_t fat16_free_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint8_t fat16_terminate_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint8_t fat16_clear_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint16_t fat16_clear_cluster_callback(uint8_t* buffer, uint32_t offset, void* p); +static uint32_t fat16_cluster_offset(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint8_t fat16_dir_entry_read_callback(uint8_t* buffer, uint32_t offset, void* p); +static uint8_t fat16_interpret_dir_entry(struct fat16_dir_entry_struct* dir_entry, const uint8_t* raw_entry); +static uint32_t fat16_find_offset_for_dir_entry(const struct fat16_fs_struct* fs, const struct fat16_dir_struct* parent, const struct fat16_dir_entry_struct* dir_entry); +static uint8_t fat16_write_dir_entry(const struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry); + +//static uint8_t fat16_get_fs_free_callback(uint8_t* buffer, uint32_t offset, void* p); + +//static void fat16_set_file_modification_date(struct fat16_dir_entry_struct* dir_entry, uint16_t year, uint8_t month, uint8_t day); +//static void fat16_set_file_modification_time(struct fat16_dir_entry_struct* dir_entry, uint8_t hour, uint8_t min, uint8_t sec); + +struct fat16_fs_struct fat16_fs_handlers[FAT16_FILE_COUNT]; + struct fat16_file_struct fat16_file_handlers[FAT16_FILE_COUNT]; + struct fat16_dir_struct fat16_dir_handlers[FAT16_DIR_COUNT]; + +/** + * \addtogroup fat16 FAT16 support + * + * This module implements FAT16 read and write access. + * + * The following features are supported: + * - File names up to 31 characters long. + * - Unlimited depth of subdirectories. + * - Short 8.3 and long filenames. + * - Creating and deleting files. + * - Reading and writing from and to files. + * - File resizing. + * - File sizes of up to 4 gigabytes. + * + * @{ + */ +/** + * \file + * FAT16 implementation (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * \addtogroup fat16_config FAT16 configuration + * Preprocessor defines to configure the FAT16 implementation. + */ + +/** + * \addtogroup fat16_fs FAT16 access + * Basic functions for handling a FAT16 filesystem. + */ + +/** + * \addtogroup fat16_file FAT16 file functions + * Functions for managing files. + */ + +/** + * \addtogroup fat16_dir FAT16 directory functions + * Functions for managing directories. + */ + +/** + * @} + */ + +#define FAT16_CLUSTER_FREE 0x0000 +#define FAT16_CLUSTER_RESERVED_MIN 0xfff0 +#define FAT16_CLUSTER_RESERVED_MAX 0xfff6 +#define FAT16_CLUSTER_BAD 0xfff7 +#define FAT16_CLUSTER_LAST_MIN 0xfff8 +#define FAT16_CLUSTER_LAST_MAX 0xffff + +#define FAT16_DIRENTRY_DELETED 0xe5 +#define FAT16_DIRENTRY_LFNLAST (1 << 6) +#define FAT16_DIRENTRY_LFNSEQMASK ((1 << 6) - 1) + +/* Each entry within the directory table has a size of 32 bytes + * and either contains a 8.3 DOS-style file name or a part of a + * long file name, which may consist of several directory table + * entries at once. + * + * multi-byte integer values are stored little-endian! + * + * 8.3 file name entry: + * ==================== + * offset length description + * 0 8 name (space padded) + * 8 3 extension (space padded) + * 11 1 attributes (FAT16_ATTRIB_*) + * + * long file name (lfn) entry ordering for a single file name: + * =========================================================== + * LFN entry n + * ... + * LFN entry 2 + * LFN entry 1 + * 8.3 entry (see above) + * + * lfn entry: + * ========== + * offset length description + * 0 1 ordinal field + * 1 2 unicode character 1 + * 3 3 unicode character 2 + * 5 3 unicode character 3 + * 7 3 unicode character 4 + * 9 3 unicode character 5 + * 11 1 attribute (always 0x0f) + * 12 1 type (reserved, always 0) + * 13 1 checksum + * 14 2 unicode character 6 + * 16 2 unicode character 7 + * 18 2 unicode character 8 + * 20 2 unicode character 9 + * 22 2 unicode character 10 + * 24 2 unicode character 11 + * 26 2 cluster (unused, always 0) + * 28 2 unicode character 12 + * 30 2 unicode character 13 + * + * The ordinal field contains a descending number, from n to 1. + * For the n'th lfn entry the ordinal field is or'ed with 0x40. + * For deleted lfn entries, the ordinal field is set to 0xe5. + */ + + + +static uint8_t fat16_read_header(struct fat16_fs_struct* fs); +static uint8_t fat16_read_root_dir_entry(const struct fat16_fs_struct* fs, uint16_t entry_num, struct fat16_dir_entry_struct* dir_entry); +static uint8_t fat16_read_sub_dir_entry(const struct fat16_fs_struct* fs, uint16_t entry_num, const struct fat16_dir_entry_struct* parent, struct fat16_dir_entry_struct* dir_entry); +static uint8_t fat16_dir_entry_seek_callback(uint8_t* buffer, uint32_t offset, void* p); +static uint8_t fat16_dir_entry_read_callback(uint8_t* buffer, uint32_t offset, void* p); +static uint8_t fat16_interpret_dir_entry(struct fat16_dir_entry_struct* dir_entry, const uint8_t* raw_entry); +static uint16_t fat16_get_next_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint16_t fat16_append_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num, uint16_t count); +static uint8_t fat16_free_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint8_t fat16_terminate_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint8_t fat16_clear_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num); +static uint16_t fat16_clear_cluster_callback(uint8_t* buffer, uint32_t offset, void* p); +static uint32_t fat16_find_offset_for_dir_entry(const struct fat16_fs_struct* fs, const struct fat16_dir_struct* parent, const struct fat16_dir_entry_struct* dir_entry); +static uint8_t fat16_write_dir_entry(const struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry); + + +/** + * \ingroup fat16_fs + * Opens a FAT16 filesystem. + * + * \param[in] partition Discriptor of partition on which the filesystem resides. + * \returns 0 on error, a FAT16 filesystem descriptor on success. + * \see fat16_open + */ +struct fat16_fs_struct* fat16_open(struct partition_struct* partition) +{ + if(!partition) + return 0; +#if USE_DYNAMIC_MEMORY + struct fat16_fs_struct* fs = malloc(sizeof(*fs)); + if(!fs) + return 0; +#else + + + struct fat16_fs_struct* fs = fat16_fs_handlers; + uint8_t i; + for(i = 0; i < FAT16_FS_COUNT; ++i) + { + if(!fs->partition) + break; + + ++fs; + } + if(i >= FAT16_FS_COUNT) + return 0; +#endif + + memset(fs, 0, sizeof(*fs)); + + fs->partition = partition; + if(!fat16_read_header(fs)) + { +#if USE_DYNAMIC_MEMORY + free(fs); +#else + fs->partition = 0; +#endif + return 0; + } + return fs; +} + +/** + * \ingroup fat16_fs + * Closes a FAT16 filesystem. + * + * When this function returns, the given filesystem descriptor + * will be invalid. + * + * \param[in] fs The filesystem to close. + * \see fat16_open + */ +void fat16_close(struct fat16_fs_struct* fs) +{ + if(!fs) + return; + +#if USE_DYNAMIC_MEMORY + free(fs); +#else + fs->partition = 0; +#endif +} + +/** + * \ingroup fat16_fs + * Reads and parses the header of a FAT16 filesystem. + * + * \param[inout] fs The filesystem for which to parse the header. + * \returns 0 on failure, 1 on success. + */ +uint8_t fat16_read_header(struct fat16_fs_struct* fs) +{ + if(!fs) + return 0; + + struct partition_struct* partition = fs->partition; + if(!partition) + return 0; + + /* read fat parameters */ + uint8_t buffer[25]; + uint32_t partition_offset = partition->offset * 512; + + if(!sd_raw_read(partition_offset + 0x0b, buffer, sizeof(buffer))) + return 0; + + uint16_t bytes_per_sector = ((uint16_t) buffer[0x00]) | + ((uint16_t) buffer[0x01] << 8); + uint8_t sectors_per_cluster = buffer[0x02]; + uint16_t reserved_sectors = ((uint16_t) buffer[0x03]) | + ((uint16_t) buffer[0x04] << 8); + uint8_t fat_copies = buffer[0x05]; + uint16_t max_root_entries = ((uint16_t) buffer[0x06]) | + ((uint16_t) buffer[0x07] << 8); + uint16_t sector_count_16 = ((uint16_t) buffer[0x08]) | + ((uint16_t) buffer[0x09] << 8); + uint16_t sectors_per_fat = ((uint16_t) buffer[0x0b]) | + ((uint16_t) buffer[0x0c] << 8); + uint32_t sector_count = ((uint32_t) buffer[0x15]) | + ((uint32_t) buffer[0x16] << 8) | + ((uint32_t) buffer[0x17] << 16) | + ((uint32_t) buffer[0x18] << 24); + + if(sectors_per_fat == 0) + /* this is not a FAT16 */ + return 0; + + if(sector_count == 0) + { + if(sector_count_16 == 0) + /* illegal volume size */ + return 0; + else + sector_count = sector_count_16; + } + + /* ensure we really have a FAT16 fs here */ + uint32_t data_sector_count = sector_count + - reserved_sectors + - (uint32_t) sectors_per_fat * fat_copies + - ((max_root_entries * 32 + bytes_per_sector - 1) / bytes_per_sector); + uint32_t data_cluster_count = data_sector_count / sectors_per_cluster; + if(data_cluster_count < 4085 || data_cluster_count >= 65525) + /* this is not a FAT16 */ + return 0; + + partition->type = PARTITION_TYPE_FAT16; + + /* fill header information */ + struct fat16_header_struct* header = &fs->header; + memset(header, 0, sizeof(*header)); + + header->size = sector_count * bytes_per_sector; + + header->fat_offset = /* jump to partition */ + partition_offset + + /* jump to fat */ + (uint32_t) reserved_sectors * bytes_per_sector; + header->fat_size = (data_cluster_count + 2) * 2; + + header->sector_size = bytes_per_sector; + header->cluster_size = (uint32_t) bytes_per_sector * sectors_per_cluster; + + header->root_dir_offset = /* jump to fats */ + header->fat_offset + + /* jump to root directory entries */ + (uint32_t) fat_copies * sectors_per_fat * bytes_per_sector; + + header->cluster_zero_offset = /* jump to root directory entries */ + header->root_dir_offset + + /* skip root directory entries */ + (uint32_t) max_root_entries * 32; + + return 1; +} + +/** + * \ingroup fat16_fs + * Reads a directory entry of the root directory. + * + * \param[in] fs Descriptor of file system to use. + * \param[in] entry_num The index of the directory entry to read. + * \param[out] dir_entry Directory entry descriptor which will get filled. + * \returns 0 on failure, 1 on success + * \see fat16_read_sub_dir_entry, fat16_read_dir_entry_by_path + */ + +uint8_t fat16_read_root_dir_entry(const struct fat16_fs_struct* fs, uint16_t entry_num, struct fat16_dir_entry_struct* dir_entry) +{ + if(!fs || !dir_entry) + return 0; + + /* we read from the root directory entry */ + const struct fat16_header_struct* header = &fs->header; + uint8_t buffer[32]; + + /* seek to the n-th entry */ + struct fat16_read_callback_arg arg; + memset(&arg, 0, sizeof(arg)); + arg.entry_num = entry_num; + if(!sd_raw_read_interval(header->root_dir_offset, + buffer, + sizeof(buffer), + header->cluster_zero_offset - header->root_dir_offset, + fat16_dir_entry_seek_callback, + &arg) || + arg.entry_offset == 0 + ) + return 0; + + /* read entry */ + memset(dir_entry, 0, sizeof(*dir_entry)); + if(!sd_raw_read_interval(arg.entry_offset, + buffer, + sizeof(buffer), + arg.byte_count, + fat16_dir_entry_read_callback, + dir_entry)) + return 0; + + return dir_entry->long_name[0] != '\0' ? 1 : 0; +} + +/** + * \ingroup fat16_fs + * Reads a directory entry of a given parent directory. + * + * \param[in] fs Descriptor of file system to use. + * \param[in] entry_num The index of the directory entry to read. + * \param[in] parent Directory entry descriptor in which to read directory entry. + * \param[out] dir_entry Directory entry descriptor which will get filled. + * \returns 0 on failure, 1 on success + * \see fat16_read_root_dir_entry, fat16_read_dir_entry_by_path + */ +uint8_t fat16_read_sub_dir_entry(const struct fat16_fs_struct* fs, uint16_t entry_num, const struct fat16_dir_entry_struct* parent, struct fat16_dir_entry_struct* dir_entry) +{ + if(!fs || !parent || !dir_entry) + return 0; + + /* we are in a parent directory and want to search within its directory entry table */ + if(!(parent->attributes & FAT16_ATTRIB_DIR)) + return 0; + + /* loop through all clusters of the directory */ + uint8_t buffer[32]; + uint32_t cluster_offset; + uint16_t cluster_size = fs->header.cluster_size; + uint16_t cluster_num = parent->cluster; + struct fat16_read_callback_arg arg; + + while(1) + { + /* calculate new cluster offset */ + cluster_offset = fs->header.cluster_zero_offset + (uint32_t) (cluster_num - 2) * cluster_size; + + /* seek to the n-th entry */ + memset(&arg, 0, sizeof(arg)); + arg.entry_num = entry_num; + if(!sd_raw_read_interval(cluster_offset, + buffer, + sizeof(buffer), + cluster_size, + fat16_dir_entry_seek_callback, + &arg) + ) + return 0; + + /* check if we found the entry */ + if(arg.entry_offset) + break; + + /* get number of next cluster */ + if(!(cluster_num = fat16_get_next_cluster(fs, cluster_num))) + return 0; /* directory entry not found */ + } + + memset(dir_entry, 0, sizeof(*dir_entry)); + + /* read entry */ + if(!sd_raw_read_interval(arg.entry_offset, + buffer, + sizeof(buffer), + arg.byte_count, + fat16_dir_entry_read_callback, + dir_entry)) + return 0; + + return dir_entry->long_name[0] != '\0' ? 1 : 0; +} + +/** + * \ingroup fat16_fs + * Callback function for seeking through subdirectory entries. + */ +uint8_t fat16_dir_entry_seek_callback(uint8_t* buffer, uint32_t offset, void* p) +{ + struct fat16_read_callback_arg* arg = (struct fat16_read_callback_arg*)p; + + /* skip deleted or empty entries */ + if(buffer[0] == FAT16_DIRENTRY_DELETED || !buffer[0]) + return 1; + + if(arg->entry_cur == arg->entry_num) + { + arg->entry_offset = offset; + arg->byte_count = buffer[11] == 0x0f ? + ((buffer[0] & FAT16_DIRENTRY_LFNSEQMASK) + 1) * 32 : + 32; + return 0; + } + + /* if we read a 8.3 entry, we reached a new directory entry */ + if(buffer[11] != 0x0f) + ++arg->entry_cur; + + return 1; +} + +/** + * \ingroup fat16_fs + * Callback function for reading a directory entry. + */ +uint8_t fat16_dir_entry_read_callback(uint8_t* buffer, uint32_t offset, void* p) +{ + struct fat16_dir_entry_struct* dir_entry = ( struct fat16_dir_entry_struct*)p; + + /* there should not be any deleted or empty entries */ + if(buffer[0] == FAT16_DIRENTRY_DELETED || !buffer[0]) + return 0; + + if(!dir_entry->entry_offset) + dir_entry->entry_offset = offset; + + switch(fat16_interpret_dir_entry(dir_entry, buffer)) + { + case 0: /* failure */ + return 0; + case 1: /* buffer successfully parsed, continue */ + return 1; + case 2: /* directory entry complete, finish */ + return 0; + } + + return 0; +} + +/** + * \ingroup fat16_fs + * Interprets a raw directory entry and puts the contained + * information into the directory entry. + * + * For a single file there may exist multiple directory + * entries. All except the last one are lfn entries, which + * contain parts of the long filename. The last directory + * entry is a traditional 8.3 style one. It contains all + * other information like size, cluster, date and time. + * + * \param[in,out] dir_entry The directory entry to fill. + * \param[in] raw_entry A pointer to 32 bytes of raw data. + * \returns 0 on failure, 1 on success and 2 if the + * directory entry is complete. + */ +uint8_t fat16_interpret_dir_entry(struct fat16_dir_entry_struct* dir_entry, const uint8_t* raw_entry) +{ + if(!dir_entry || !raw_entry || !raw_entry[0]) + return 0; + + char* long_name = dir_entry->long_name; + if(raw_entry[11] == 0x0f) + { + uint16_t char_offset = ((raw_entry[0] & 0x3f) - 1) * 13; + + if(char_offset + 12 < sizeof(dir_entry->long_name)) + { + /* Lfn supports unicode, but we do not, for now. + * So we assume pure ascii and read only every + * second byte. + */ + long_name[char_offset + 0] = raw_entry[1]; + long_name[char_offset + 1] = raw_entry[3]; + long_name[char_offset + 2] = raw_entry[5]; + long_name[char_offset + 3] = raw_entry[7]; + long_name[char_offset + 4] = raw_entry[9]; + long_name[char_offset + 5] = raw_entry[14]; + long_name[char_offset + 6] = raw_entry[16]; + long_name[char_offset + 7] = raw_entry[18]; + long_name[char_offset + 8] = raw_entry[20]; + long_name[char_offset + 9] = raw_entry[22]; + long_name[char_offset + 10] = raw_entry[24]; + long_name[char_offset + 11] = raw_entry[28]; + long_name[char_offset + 12] = raw_entry[30]; + } + + return 1; + } + else + { + /* if we do not have a long name, take the short one */ + if(long_name[0] == '\0') + { + uint8_t i; + for(i = 0; i < 8; ++i) + { + if(raw_entry[i] == ' ') + break; + long_name[i] = raw_entry[i]; + } + if(long_name[0] == 0x05) + long_name[0] = (char) FAT16_DIRENTRY_DELETED; + + if(raw_entry[8] != ' ') + { + long_name[i++] = '.'; + + uint8_t j = 8; + for(; j < 11; ++j) + { + if(raw_entry[j] != ' ') + { + long_name[i++] = raw_entry[j]; + } + else + { + break; + } + } + } + + long_name[i] = '\0'; + } + + /* extract properties of file and store them within the structure */ + dir_entry->attributes = raw_entry[11]; + dir_entry->cluster = ((uint16_t) raw_entry[26]) | + ((uint16_t) raw_entry[27] << 8); + dir_entry->file_size = ((uint32_t) raw_entry[28]) | + ((uint32_t) raw_entry[29] << 8) | + ((uint32_t) raw_entry[30] << 16) | + ((uint32_t) raw_entry[31] << 24); + +#if FAT16_DATETIME_SUPPORT + dir_entry->modification_time = ((uint16_t) raw_entry[22]) | + ((uint16_t) raw_entry[23] << 8); + dir_entry->modification_date = ((uint16_t) raw_entry[24]) | + ((uint16_t) raw_entry[25] << 8); +#endif + + return 2; + } +} + +/** + * \ingroup fat16_file + * Retrieves the directory entry of a path. + * + * The given path may both describe a file or a directory. + * + * \param[in] fs The FAT16 filesystem on which to search. + * \param[in] path The path of which to read the directory entry. + * \param[out] dir_entry The directory entry to fill. + * \returns 0 on failure, 1 on success. + * \see fat16_read_dir + */ +uint8_t fat16_get_dir_entry_of_root(struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry) +{ + if(!fs || !dir_entry) + return 0; + /* begin with the root directory */ + memset(dir_entry, 0, sizeof(*dir_entry)); + dir_entry->attributes = FAT16_ATTRIB_DIR; + return 1; +} + +/** + * \ingroup fat16_fs + * Retrieves the next following cluster of a given cluster. + * + * Using the filesystem file allocation table, this function returns + * the number of the cluster containing the data directly following + * the data within the cluster with the given number. + * + * \param[in] fs The filesystem for which to determine the next cluster. + * \param[in] cluster_num The number of the cluster for which to determine its successor. + * \returns The wanted cluster number, or 0 on error. + */ +uint16_t fat16_get_next_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num) +{ + if(!fs || cluster_num < 2) + return 0; + + /* read appropriate fat entry */ + uint8_t fat_entry[2]; + if(!sd_raw_read(fs->header.fat_offset + 2 * cluster_num, fat_entry, 2)) + return 0; + + /* determine next cluster from fat */ + cluster_num = ((uint16_t) fat_entry[0]) | + ((uint16_t) fat_entry[1] << 8); + + if(cluster_num == FAT16_CLUSTER_FREE || + cluster_num == FAT16_CLUSTER_BAD || + (cluster_num >= FAT16_CLUSTER_RESERVED_MIN && cluster_num <= FAT16_CLUSTER_RESERVED_MAX) || + (cluster_num >= FAT16_CLUSTER_LAST_MIN && cluster_num <= FAT16_CLUSTER_LAST_MAX)) + return 0; + + return cluster_num; +} + +/** + * \ingroup fat16_fs + * Appends a new cluster chain to an existing one. + * + * Set cluster_num to zero to create a completely new one. + * + * \param[in] fs The file system on which to operate. + * \param[in] cluster_num The cluster to which to append the new chain. + * \param[in] count The number of clusters to allocate. + * \returns 0 on failure, the number of the first new cluster on success. + */ +uint16_t fat16_append_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num, uint16_t count) +{ +#if FAT16_WRITE_SUPPORT + if(!fs) + return 0; + + uint32_t fat_offset = fs->header.fat_offset; + uint16_t cluster_max = fs->header.fat_size / 2; + uint16_t cluster_next = 0; + uint16_t count_left = count; + uint8_t buffer[2]; + uint16_t cluster_new; + for(cluster_new = 0; cluster_new < cluster_max; ++cluster_new) + { + if(!sd_raw_read(fat_offset + 2 * cluster_new, buffer, sizeof(buffer))) + return 0; + + /* check if this is a free cluster */ + if(buffer[0] == (FAT16_CLUSTER_FREE & 0xff) && + buffer[1] == ((FAT16_CLUSTER_FREE >> 8) & 0xff)) + { + /* allocate cluster */ + if(count_left == count) + { + buffer[0] = FAT16_CLUSTER_LAST_MAX & 0xff; + buffer[1] = (FAT16_CLUSTER_LAST_MAX >> 8) & 0xff; + } + else + { + buffer[0] = cluster_next & 0xff; + buffer[1] = (cluster_next >> 8) & 0xff; + } + + if(!sd_raw_write(fat_offset + 2 * cluster_new, buffer, sizeof(buffer))) + break; + + cluster_next = cluster_new; + if(--count_left == 0) + break; + } + } + + do + { + if(count_left > 0) + break; + + /* We allocated a new cluster chain. Now join + * it with the existing one. + */ + if(cluster_num >= 2) + { + buffer[0] = cluster_next & 0xff; + buffer[1] = (cluster_next >> 8) & 0xff; + if(!sd_raw_write(fat_offset + 2 * cluster_num, buffer, sizeof(buffer))) + break; + } + + return cluster_next; + + } while(0); + + /* No space left on device or writing error. + * Free up all clusters already allocated. + */ + fat16_free_clusters(fs, cluster_next); + + return 0; +#else + return 0; +#endif +} + +/** + * \ingroup fat16_fs + * Frees a cluster chain, or a part thereof. + * + * Marks the specified cluster and all clusters which are sequentially + * referenced by it as free. They may then be used again for future + * file allocations. + * + * \note If this function is used for freeing just a part of a cluster + * chain, the new end of the chain is not correctly terminated + * within the FAT. Use fat16_terminate_clusters() instead. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] cluster_num The starting cluster of the chain which to free. + * \returns 0 on failure, 1 on success. + * \see fat16_terminate_clusters + */ +uint8_t fat16_free_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num) +{ +#if FAT16_WRITE_SUPPORT + if(!fs || cluster_num < 2) + return 0; + + uint32_t fat_offset = fs->header.fat_offset; + uint8_t buffer[2]; + while(cluster_num) + { + if(!sd_raw_read(fat_offset + 2 * cluster_num, buffer, 2)) + return 0; + + /* get next cluster of current cluster before freeing current cluster */ + uint16_t cluster_num_next = ((uint16_t) buffer[0]) | + ((uint16_t) buffer[1] << 8); + + if(cluster_num_next == FAT16_CLUSTER_FREE) + return 1; + if(cluster_num_next == FAT16_CLUSTER_BAD || + (cluster_num_next >= FAT16_CLUSTER_RESERVED_MIN && + cluster_num_next <= FAT16_CLUSTER_RESERVED_MAX + ) + ) + return 0; + if(cluster_num_next >= FAT16_CLUSTER_LAST_MIN && cluster_num_next <= FAT16_CLUSTER_LAST_MAX) + cluster_num_next = 0; + + /* free cluster */ + buffer[0] = FAT16_CLUSTER_FREE & 0xff; + buffer[1] = (FAT16_CLUSTER_FREE >> 8) & 0xff; + sd_raw_write(fat_offset + 2 * cluster_num, buffer, 2); + + /* We continue in any case here, even if freeing the cluster failed. + * The cluster is lost, but maybe we can still free up some later ones. + */ + + cluster_num = cluster_num_next; + } + + return 1; +#else + return 0; +#endif +} + +/** + * \ingroup fat16_fs + * Frees a part of a cluster chain and correctly terminates the rest. + * + * Marks the specified cluster as the new end of a cluster chain and + * frees all following clusters. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] cluster_num The new end of the cluster chain. + * \returns 0 on failure, 1 on success. + * \see fat16_free_clusters + */ +uint8_t fat16_terminate_clusters(const struct fat16_fs_struct* fs, uint16_t cluster_num) +{ +#if FAT16_WRITE_SUPPORT + if(!fs || cluster_num < 2) + return 0; + + /* fetch next cluster before overwriting the cluster entry */ + uint16_t cluster_num_next = fat16_get_next_cluster(fs, cluster_num); + + /* mark cluster as the last one */ + uint8_t buffer[2]; + buffer[0] = FAT16_CLUSTER_LAST_MAX & 0xff; + buffer[1] = (FAT16_CLUSTER_LAST_MAX >> 8) & 0xff; + if(!sd_raw_write(fs->header.fat_offset + 2 * cluster_num, buffer, 2)) + return 0; + + /* free remaining clusters */ + if(cluster_num_next) + return fat16_free_clusters(fs, cluster_num_next); + else + return 1; +#else + return 0; +#endif +} + +/** + * \ingroup fat16_fs + * Clears a single cluster. + * + * The complete cluster is filled with zeros. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] cluster_num The cluster to clear. + * \returns 0 on failure, 1 on success. + */ +uint8_t fat16_clear_cluster(const struct fat16_fs_struct* fs, uint16_t cluster_num) +{ +#if FAT16_WRITE_SUPPORT + if(cluster_num < 2) + return 0; + + uint32_t cluster_offset = fs->header.cluster_zero_offset + + (uint32_t) (cluster_num - 2) * fs->header.cluster_size; + uint8_t zero[16]; + return sd_raw_write_interval(cluster_offset, + zero, + fs->header.cluster_size, + fat16_clear_cluster_callback, + 0 + ); +#else + return 0; +#endif +} + +/** + * \ingroup fat16_fs + * Callback function for clearing a cluster. + */ +uint16_t fat16_clear_cluster_callback(uint8_t* buffer, uint32_t offset, void* p) +{ +#if FAT16_WRITE_SUPPORT + memset(buffer, 0, 16); + return 16; +#else + return 0; +#endif +} + +/** + * \ingroup fat16_file + * Opens a file on a FAT16 filesystem. + * + * \param[in] fs The filesystem on which the file to open lies. + * \param[in] dir_entry The directory entry of the file to open. + * \returns The file handle, or 0 on failure. + * \see fat16_close_file + */ +struct fat16_file_struct* fat16_open_file(struct fat16_fs_struct* fs, const struct fat16_dir_entry_struct* dir_entry) +{ + if(!fs || !dir_entry || (dir_entry->attributes & FAT16_ATTRIB_DIR)) + return 0; + +#if USE_DYNAMIC_MEMORY + struct fat16_file_struct* fd = malloc(sizeof(*fd)); + if(!fd) + return 0; +#else + struct fat16_file_struct* fd = fat16_file_handlers; + uint8_t i; + for(i = 0; i < FAT16_FILE_COUNT; ++i) + { + if(!fd->fs) + break; + + ++fd; + } +#endif + + memcpy(&fd->dir_entry, dir_entry, sizeof(*dir_entry)); + fd->fs = fs; + fd->pos = 0; + fd->pos_cluster = dir_entry->cluster; + + return fd; +} + +/** + * \ingroup fat16_file + * Closes a file. + * + * \param[in] fd The file handle of the file to close. + * \see fat16_open_file + */ +void fat16_close_file(struct fat16_file_struct* fd) +{ + if(fd) +#if USE_DYNAMIC_MEMORY + free(fd); +#else + fd->fs = 0; +#endif +} + +/** + * \ingroup fat16_file + * Reads data from a file. + * + * The data requested is read from the current file location. + * + * \param[in] fd The file handle of the file from which to read. + * \param[out] buffer The buffer into which to write. + * \param[in] buffer_len The amount of data to read. + * \returns The number of bytes read, 0 on end of file, or -1 on failure. + * \see fat16_write_file + */ +int16_t fat16_read_file(struct fat16_file_struct* fd, uint8_t* buffer, uint16_t buffer_len) +{ + /* check arguments */ + if(!fd || !buffer || buffer_len < 1) + return -1; + + /* determine number of bytes to read */ + if(fd->pos + buffer_len > fd->dir_entry.file_size) + buffer_len = fd->dir_entry.file_size - fd->pos; + if(buffer_len == 0) + return 0; + + uint16_t cluster_size = fd->fs->header.cluster_size; + uint16_t cluster_num = fd->pos_cluster; + uint16_t buffer_left = buffer_len; + uint16_t first_cluster_offset = fd->pos % cluster_size; + + /* find cluster in which to start reading */ + if(!cluster_num) + { + cluster_num = fd->dir_entry.cluster; + + if(!cluster_num) + { + if(!fd->pos) + return 0; + else + return -1; + } + + if(fd->pos) + { + uint32_t pos = fd->pos; + while(pos >= cluster_size) + { + pos -= cluster_size; + cluster_num = fat16_get_next_cluster(fd->fs, cluster_num); + if(!cluster_num) + return -1; + } + } + } + + /* read data */ + do + { + /* calculate data size to copy from cluster */ + uint32_t cluster_offset = fat16_cluster_offset(fd->fs, cluster_num) + first_cluster_offset; + uint16_t copy_length = cluster_size - first_cluster_offset; + if(copy_length > buffer_left) + copy_length = buffer_left; + + /* read data */ + if(!fd->fs->partition->device_read(cluster_offset, buffer, copy_length)) + return buffer_len - buffer_left; + + /* calculate new file position */ + buffer += copy_length; + buffer_left -= copy_length; + fd->pos += copy_length; + + if(first_cluster_offset + copy_length >= cluster_size) + { + /* we are on a cluster boundary, so get the next cluster */ + if((cluster_num = fat16_get_next_cluster(fd->fs, cluster_num))) + { + first_cluster_offset = 0; + } + else + { + fd->pos_cluster = 0; + return buffer_len - buffer_left; + } + } + + fd->pos_cluster = cluster_num; + + } while(buffer_left > 0); /* check if we are done */ + + return buffer_len; +} + + +/** + * \ingroup fat16_file + * Writes data to a file. + * + * The data is written to the current file location. + * + * \param[in] fd The file handle of the file to which to write. + * \param[in] buffer The buffer from which to read the data to be written. + * \param[in] buffer_len The amount of data to write. + * \returns The number of bytes written, 0 on disk full, or -1 on failure. + * \see fat16_read_file + */ +int16_t fat16_write_file(struct fat16_file_struct* fd, const uint8_t* buffer, uint16_t buffer_len) +{ +#if FAT16_WRITE_SUPPORT + /* check arguments */ + if(!fd || !buffer || buffer_len < 1) + return -1; + if(fd->pos > fd->dir_entry.file_size) + return -1; + + uint16_t cluster_size = fd->fs->header.cluster_size; + uint16_t cluster_num = fd->pos_cluster; + uint16_t buffer_left = buffer_len; + uint16_t first_cluster_offset = fd->pos % cluster_size; + + /* find cluster in which to start writing */ + if(!cluster_num) + { + cluster_num = fd->dir_entry.cluster; + + if(!cluster_num) + { + if(!fd->pos) + { + /* empty file */ + fd->dir_entry.cluster = cluster_num = fat16_append_clusters(fd->fs, 0, 1); + if(!cluster_num) + return -1; + } + else + { + return -1; + } + } + + if(fd->pos) + { + uint32_t pos = fd->pos; + uint16_t cluster_num_next; + while(pos >= cluster_size) + { + pos -= cluster_size; + cluster_num_next = fat16_get_next_cluster(fd->fs, cluster_num); + if(!cluster_num_next && pos == 0) + /* the file exactly ends on a cluster boundary, and we append to it */ + cluster_num_next = fat16_append_clusters(fd->fs, cluster_num, 1); + if(!cluster_num_next) + return -1; + + cluster_num = cluster_num_next; + } + } + } + + /* write data */ + do + { + /* calculate data size to write to cluster */ + uint32_t cluster_offset = fd->fs->header.cluster_zero_offset + + (uint32_t) (cluster_num - 2) * cluster_size + first_cluster_offset; + uint16_t write_length = cluster_size - first_cluster_offset; + if(write_length > buffer_left) + write_length = buffer_left; + + /* write data which fits into the current cluster */ + if(!sd_raw_write(cluster_offset, buffer, write_length)) + break; + + /* calculate new file position */ + buffer += write_length; + buffer_left -= write_length; + fd->pos += write_length; + + if(first_cluster_offset + write_length >= cluster_size) + { + /* we are on a cluster boundary, so get the next cluster */ + uint16_t cluster_num_next = fat16_get_next_cluster(fd->fs, cluster_num); + if(!cluster_num_next && buffer_left > 0) + /* we reached the last cluster, append a new one */ + cluster_num_next = fat16_append_clusters(fd->fs, cluster_num, 1); + if(!cluster_num_next) + { + fd->pos_cluster = 0; + break; + } + + cluster_num = cluster_num_next; + first_cluster_offset = 0; + } + + fd->pos_cluster = cluster_num; + + } while(buffer_left > 0); /* check if we are done */ + + /* update directory entry */ + if(fd->pos > fd->dir_entry.file_size) + { + uint32_t size_old = fd->dir_entry.file_size; + + /* update file size */ + fd->dir_entry.file_size = fd->pos; + /* write directory entry */ + if(!fat16_write_dir_entry(fd->fs, &fd->dir_entry)) + { + /* We do not return an error here since we actually wrote + * some data to disk. So we calculate the amount of data + * we wrote to disk and which lies within the old file size. + */ + buffer_left = fd->pos - size_old; + fd->pos = size_old; + } + } + + return buffer_len - buffer_left; + +#else + return -1; +#endif +} + +/** + * \ingroup fat16_file + * Repositions the read/write file offset. + * + * Changes the file offset where the next call to fat16_read_file() + * or fat16_write_file() starts reading/writing. + * + * If the new offset is beyond the end of the file, fat16_resize_file() + * is implicitly called, i.e. the file is expanded. + * + * The new offset can be given in different ways determined by + * the \c whence parameter: + * - \b FAT16_SEEK_SET: \c *offset is relative to the beginning of the file. + * - \b FAT16_SEEK_CUR: \c *offset is relative to the current file position. + * - \b FAT16_SEEK_END: \c *offset is relative to the end of the file. + * + * The resulting absolute offset is written to the location the \c offset + * parameter points to. + * + * \param[in] fd The file decriptor of the file on which to seek. + * \param[in,out] offset A pointer to the new offset, as affected by the \c whence + * parameter. The function writes the new absolute offset + * to this location before it returns. + * \param[in] whence Affects the way \c offset is interpreted, see above. + * \returns 0 on failure, 1 on success. + */ + +uint8_t fat16_seek_file(struct fat16_file_struct* fd, int32_t* offset, uint8_t whence) +{ + if(!fd || !offset) + return 0; + + uint32_t new_pos = fd->pos; + switch(whence) + { + case FAT16_SEEK_SET: + new_pos = *offset; + break; + case FAT16_SEEK_CUR: + new_pos += *offset; + break; + case FAT16_SEEK_END: + new_pos = fd->dir_entry.file_size + *offset; + break; + default: + return 0; + } + + if(new_pos > fd->dir_entry.file_size) + return 0; + + fd->pos = new_pos; + fd->pos_cluster = 0; + + *offset = new_pos; + return 1; +} + +/** + * \ingroup fat16_dir + * Opens a directory. + * + * \param[in] fs The filesystem on which the directory to open resides. + * \param[in] dir_entry The directory entry which stands for the directory to open. + * \returns An opaque directory descriptor on success, 0 on failure. + * \see fat16_close_dir + */ +struct fat16_dir_struct* fat16_open_dir(struct fat16_fs_struct* fs, const struct fat16_dir_entry_struct* dir_entry) +{ + if(!fs || !dir_entry || !(dir_entry->attributes & FAT16_ATTRIB_DIR)) + return 0; + +#if USE_DYNAMIC_MEMORY + struct fat16_dir_struct* dd = malloc(sizeof(*dd)); + if(!dd) + return 0; +#else + struct fat16_dir_struct* dd = fat16_dir_handlers; + uint8_t i; + for(i = 0; i < FAT16_DIR_COUNT; ++i) + { + if(!dd->fs) + break; + + ++dd; + } + if(i >= FAT16_DIR_COUNT) + return 0; +#endif + + memcpy(&dd->dir_entry, dir_entry, sizeof(*dir_entry)); + dd->fs = fs; + dd->entry_next = 0; + + return dd; +} + +/** + * \ingroup fat16_dir + * Closes a directory descriptor. + * + * This function destroys a directory descriptor which was + * previously obtained by calling fat16_open_dir(). When this + * function returns, the given descriptor will be invalid. + * + * \param[in] dd The directory descriptor to close. + * \see fat16_open_dir + */ +void fat16_close_dir(struct fat16_dir_struct* dd) +{ + if(dd) +#if USE_DYNAMIC_MEMORY + free(dd); +#else + dd->fs = 0; +#endif +} + +/** + * \ingroup fat16_dir + * Reads the next directory entry contained within a parent directory. + * + * \param[in] dd The descriptor of the parent directory from which to read the entry. + * \param[out] dir_entry Pointer to a buffer into which to write the directory entry information. + * \returns 0 on failure, 1 on success. + * \see fat16_reset_dir + */ +uint8_t fat16_read_dir(struct fat16_dir_struct* dd, struct fat16_dir_entry_struct* dir_entry) +{ + if(!dd || !dir_entry) + return 0; + + if(dd->dir_entry.cluster == 0) + { + /* read entry from root directory */ + if(fat16_read_root_dir_entry(dd->fs, dd->entry_next, dir_entry)) + { + ++dd->entry_next; + return 1; + } + } + else + { + /* read entry from a subdirectory */ + if(fat16_read_sub_dir_entry(dd->fs, dd->entry_next, &dd->dir_entry, dir_entry)) + { + ++dd->entry_next; + return 1; + } + } + + /* restart reading */ + dd->entry_next = 0; + + return 0; +} + +/** + * \ingroup fat16_dir + * Resets a directory handle. + * + * Resets the directory handle such that reading restarts + * with the first directory entry. + * + * \param[in] dd The directory handle to reset. + * \returns 0 on failure, 1 on success. + * \see fat16_read_dir + */ +uint8_t fat16_reset_dir(struct fat16_dir_struct* dd) +{ + if(!dd) + return 0; + + dd->entry_next = 0; + return 1; +} + +/** + * \ingroup fat16_fs + * Searches for space where to store a directory entry. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] dir_entry The directory entry for which to search space. + * \returns 0 on failure, a device offset on success. + */ +uint32_t fat16_find_offset_for_dir_entry(const struct fat16_fs_struct* fs, const struct fat16_dir_struct* parent, const struct fat16_dir_entry_struct* dir_entry) +{ +#if FAT16_WRITE_SUPPORT + if(!fs || !dir_entry) + return 0; + + /* search for a place where to write the directory entry to disk */ + uint8_t free_dir_entries_needed = (strlen(dir_entry->long_name) + 12) / 13 + 1; + uint8_t free_dir_entries_found = 0; + uint16_t cluster_num = parent->dir_entry.cluster; + uint32_t dir_entry_offset = 0; + uint32_t offset = 0; + uint32_t offset_to = 0; + + if(cluster_num == 0) + { + /* we read/write from the root directory entry */ + offset = fs->header.root_dir_offset; + offset_to = fs->header.cluster_zero_offset; + dir_entry_offset = offset; + } + + while(1) + { + if(offset == offset_to) + { + if(cluster_num == 0) + /* We iterated through the whole root directory entry + * and could not find enough space for the directory entry. + */ + return 0; + + if(offset) + { + /* We reached a cluster boundary and have to + * switch to the next cluster. + */ + + uint16_t cluster_next = fat16_get_next_cluster(fs, cluster_num); + if(!cluster_next) + { + cluster_next = fat16_append_clusters(fs, cluster_num, 1); + if(!cluster_next) + return 0; + + /* we appended a new cluster and know it is free */ + dir_entry_offset = fs->header.cluster_zero_offset + + (uint32_t) (cluster_next - 2) * fs->header.cluster_size; + + /* clear cluster to avoid garbage directory entries */ + fat16_clear_cluster(fs, cluster_next); + + break; + } + cluster_num = cluster_next; + } + + offset = fs->header.cluster_zero_offset + + (uint32_t) (cluster_num - 2) * fs->header.cluster_size; + offset_to = offset + fs->header.cluster_size; + dir_entry_offset = offset; + free_dir_entries_found = 0; + } + + /* read next lfn or 8.3 entry */ + uint8_t first_char; + if(!sd_raw_read(offset, &first_char, sizeof(first_char))) + return 0; + + /* check if we found a free directory entry */ + if(first_char == FAT16_DIRENTRY_DELETED || !first_char) + { + /* check if we have the needed number of available entries */ + ++free_dir_entries_found; + if(free_dir_entries_found >= free_dir_entries_needed) + break; + + offset += 32; + } + else + { + offset += 32; + dir_entry_offset = offset; + free_dir_entries_found = 0; + } + } + + return dir_entry_offset; + +#else + return 0; +#endif +} + +/** + * \ingroup fat16_fs + * Writes a directory entry to disk. + * + * \note The file name is not checked for invalid characters. + * + * \note The generation of the short 8.3 file name is quite + * simple. The first eight characters are used for the filename. + * The extension, if any, is made up of the first three characters + * following the last dot within the long filename. If the + * filename (without the extension) is longer than eight characters, + * the lower byte of the cluster number replaces the last two + * characters to avoid name clashes. In any other case, it is your + * responsibility to avoid name clashes. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] dir_entry The directory entry to write. + * \returns 0 on failure, 1 on success. + */ +uint8_t fat16_write_dir_entry(const struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry) +{ +#if FAT16_WRITE_SUPPORT + if(!fs || !dir_entry) + return 0; + +#if FAT16_DATETIME_SUPPORT + { + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + + fat16_get_datetime(&year, &month, &day, &hour, &min, &sec); + fat16_set_file_modification_date(dir_entry, year, month, day); + fat16_set_file_modification_time(dir_entry, hour, min, sec); + } +#endif + + uint32_t offset = dir_entry->entry_offset; + const char* name = dir_entry->long_name; + uint8_t name_len = strlen(name); + uint8_t buffer[32]; + uint8_t i; + + /* write 8.3 entry */ + + /* generate 8.3 file name */ + memset(&buffer[0], ' ', 11); + char* name_ext = strrchr(name, '.'); + if(name_ext && *++name_ext) + { + uint8_t name_ext_len = strlen(name_ext); + name_len -= name_ext_len + 1; + + if(name_ext_len > 3) + name_ext_len = 3; + + memcpy(&buffer[8], name_ext, name_ext_len); + } + + if(name_len <= 8) + { + memcpy(buffer, name, name_len); + } + else + { + memcpy(buffer, name, 8); + + /* Minimize 8.3 name clashes by appending + * the lower byte of the cluster number. + */ + uint8_t num = dir_entry->cluster & 0xff; + + buffer[6] = (num < 0xa0) ? ('0' + (num >> 4)) : ('a' + (num >> 4)); + num &= 0x0f; + buffer[7] = (num < 0x0a) ? ('0' + num) : ('a' + num); + } + if(buffer[0] == FAT16_DIRENTRY_DELETED) + buffer[0] = 0x05; + + /* fill directory entry buffer */ + memset(&buffer[11], 0, sizeof(buffer) - 11); + buffer[0x0b] = dir_entry->attributes; +#if FAT16_DATETIME_SUPPORT + buffer[0x16] = (dir_entry->modification_time >> 0) & 0xff; + buffer[0x17] = (dir_entry->modification_time >> 8) & 0xff; + buffer[0x18] = (dir_entry->modification_date >> 0) & 0xff; + buffer[0x19] = (dir_entry->modification_date >> 8) & 0xff; +#endif + buffer[0x1a] = (dir_entry->cluster >> 0) & 0xff; + buffer[0x1b] = (dir_entry->cluster >> 8) & 0xff; + buffer[0x1c] = (dir_entry->file_size >> 0) & 0xff; + buffer[0x1d] = (dir_entry->file_size >> 8) & 0xff; + buffer[0x1e] = (dir_entry->file_size >> 16) & 0xff; + buffer[0x1f] = (dir_entry->file_size >> 24) & 0xff; + + /* write to disk */ + if(!sd_raw_write(offset, buffer, sizeof(buffer))) + return 0; + + /* calculate checksum of 8.3 name */ + uint8_t checksum = buffer[0]; + for(i = 1; i < 11; ++i) + checksum = ((checksum >> 1) | (checksum << 7)) + buffer[i]; + + return 1; + +#else + return 0; +#endif +} + +/** + * \ingroup fat16_file + * Creates a file. + * + * Creates a file and obtains the directory entry of the + * new file. If the file to create already exists, the + * directory entry of the existing file will be returned + * within the dir_entry parameter. + * + * \note The file name is not checked for invalid characters. + * + * \note The generation of the short 8.3 file name is quite + * simple. The first eight characters are used for the filename. + * The extension, if any, is made up of the first three characters + * following the last dot within the long filename. If the + * filename (without the extension) is longer than eight characters, + * the lower byte of the cluster number replaces the last two + * characters to avoid name clashes. In any other case, it is your + * responsibility to avoid name clashes. + * + * \param[in] parent The handle of the directory in which to create the file. + * \param[in] file The name of the file to create. + * \param[out] dir_entry The directory entry to fill for the new file. + * \returns 0 on failure, 1 on success. + * \see fat16_delete_file + */ +uint8_t fat16_create_file(struct fat16_dir_struct* parent, const char* file, struct fat16_dir_entry_struct* dir_entry) +{ +#if FAT16_WRITE_SUPPORT + if(!parent || !file || !file[0] || !dir_entry) + return 0; + + /* check if the file already exists */ + while(1) + { + if(!fat16_read_dir(parent, dir_entry)) + break; + + if(strcmp(file, dir_entry->long_name) == 0) + { + fat16_reset_dir(parent); + return 0; + } + } + + struct fat16_fs_struct* fs = parent->fs; + + /* prepare directory entry with values already known */ + memset(dir_entry, 0, sizeof(*dir_entry)); + strncpy(dir_entry->long_name, file, sizeof(dir_entry->long_name) - 1); + + /* find place where to store directory entry */ + if(!(dir_entry->entry_offset = fat16_find_offset_for_dir_entry(fs, parent, dir_entry))) + return 0; + + /* write directory entry to disk */ + if(!fat16_write_dir_entry(fs, dir_entry)) + return 0; + + return 1; + +#else + return 0; +#endif +} +/** + * \ingroup fat16_fs + * Returns the amount of total storage capacity of the filesystem in bytes. + * + * \param[in] fs The filesystem on which to operate. + * \returns 0 on failure, the filesystem size in bytes otherwise. + */ +uint32_t fat16_get_fs_size(const struct fat16_fs_struct* fs) +{ + if(!fs) + return 0; + + return (fs->header.fat_size / 2 - 2) * fs->header.cluster_size; +} + +/** + * \ingroup fat16_fs + * Returns the amount of free storage capacity on the filesystem in bytes. + * + * \note As the FAT16 filesystem is cluster based, this function does not + * return continuous values but multiples of the cluster size. + * + * \param[in] fs The filesystem on which to operate. + * \returns 0 on failure, the free filesystem space in bytes otherwise. + */ +uint32_t fat16_get_fs_free(const struct fat16_fs_struct* fs) +{ + if(!fs) + return 0; + + uint8_t fat[32]; + struct fat16_usage_count_callback_arg count_arg; + count_arg.cluster_count = 0; + count_arg.buffer_size = sizeof(fat); + + uint32_t fat_offset = fs->header.fat_offset; + uint32_t fat_size = fs->header.fat_size; + while(fat_size > 0) + { + uint16_t length = UINT16_MAX - 1; + if(fat_size < length) + length = fat_size; + + if(!sd_raw_read_interval(fat_offset, + fat, + sizeof(fat), + length, + fat16_get_fs_free_callback, + &count_arg + ) + ) + return 0; + + fat_offset += length; + fat_size -= length; + } + + return (uint32_t) count_arg.cluster_count * fs->header.cluster_size; +} + +/** + * \ingroup fat16_fs + * Callback function used for counting free clusters. + */ +uint8_t fat16_get_fs_free_callback(uint8_t* buffer, uint32_t offset, void* p) +{ + struct fat16_usage_count_callback_arg* count_arg = (struct fat16_usage_count_callback_arg*) p; + uint8_t buffer_size = count_arg->buffer_size; + uint8_t i; + + for(i = 0; i < buffer_size; i += 2) + { + if((((uint16_t) buffer[1] << 8) | ((uint16_t) buffer[0] << 0)) == FAT16_CLUSTER_FREE) + ++(count_arg->cluster_count); + + buffer += 2; + } + + return 1; +} + +/** + * \ingroup fat16_fs + * Calculates the offset of the specified cluster. + * + * \param[in] fs The filesystem on which to operate. + * \param[in] cluster_num The cluster whose offset to calculate. + * \returns The cluster offset. + */ +uint32_t fat16_cluster_offset(const struct fat16_fs_struct* fs, uint16_t cluster_num) +{ + if(!fs || cluster_num < 2) + return 0; + + return fs->header.cluster_zero_offset + (uint32_t) (cluster_num - 2) * fs->header.cluster_size; +} diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.h new file mode 100755 index 00000000..c77d5eec --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16.h @@ -0,0 +1,175 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef FAT16_H +#define FAT16_H + +#include "fat16_config.h" + +#include <stdint.h> + +/** + * \addtogroup fat16 + * + * @{ + */ +/** + * \file + * FAT16 header (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * \addtogroup fat16_file + * @{ + */ + +/** The file is read-only. */ +#define FAT16_ATTRIB_READONLY (1 << 0) +/** The file is hidden. */ +#define FAT16_ATTRIB_HIDDEN (1 << 1) +/** The file is a system file. */ +#define FAT16_ATTRIB_SYSTEM (1 << 2) +/** The file is empty and has the volume label as its name. */ +#define FAT16_ATTRIB_VOLUME (1 << 3) +/** The file is a directory. */ +#define FAT16_ATTRIB_DIR (1 << 4) +/** The file has to be archived. */ +#define FAT16_ATTRIB_ARCHIVE (1 << 5) + +/** The given offset is relative to the beginning of the file. */ +#define FAT16_SEEK_SET 0 +/** The given offset is relative to the current read/write position. */ +#define FAT16_SEEK_CUR 1 +/** The given offset is relative to the end of the file. */ +#define FAT16_SEEK_END 2 + +/** + * @} + */ + +struct partition_struct; +struct fat16_fs_struct; +struct fat16_file_struct; +struct fat16_dir_struct; + +/** + * \ingroup fat16_file + * Describes a directory entry. + */ +struct fat16_dir_entry_struct +{ + /** The file's name, truncated to 13 characters. */ + char long_name[13]; // 8.3+1 null + /** The file's attributes. Mask of the FAT16_ATTRIB_* constants. */ + uint8_t attributes; +#if FAT16_DATETIME_SUPPORT + /** Compressed representation of modification time. */ + uint16_t modification_time; + /** Compressed representation of modification date. */ + uint16_t modification_date; +#endif + /** The cluster in which the file's first byte resides. */ + uint16_t cluster; + /** The file's size. */ + uint32_t file_size; + /** The total disk offset of this directory entry. */ + uint32_t entry_offset; +}; + +struct fat16_fs_struct* fat16_open(struct partition_struct* partition); +void fat16_close(struct fat16_fs_struct* fs); + +struct fat16_file_struct* fat16_open_file(struct fat16_fs_struct* fs, const struct fat16_dir_entry_struct* dir_entry); +void fat16_close_file(struct fat16_file_struct* fd); +int16_t fat16_read_file(struct fat16_file_struct* fd, uint8_t* buffer, uint16_t buffer_len); +int16_t fat16_write_file(struct fat16_file_struct* fd, const uint8_t* buffer, uint16_t buffer_len); +uint8_t fat16_seek_file(struct fat16_file_struct* fd, int32_t* offset, uint8_t whence); +uint8_t fat16_resize_file(struct fat16_file_struct* fd, uint32_t size); + +struct fat16_dir_struct* fat16_open_dir(struct fat16_fs_struct* fs, const struct fat16_dir_entry_struct* dir_entry); +void fat16_close_dir(struct fat16_dir_struct* dd); +uint8_t fat16_read_dir(struct fat16_dir_struct* dd, struct fat16_dir_entry_struct* dir_entry); +uint8_t fat16_reset_dir(struct fat16_dir_struct* dd); + +uint8_t fat16_create_file(struct fat16_dir_struct* parent, const char* file, struct fat16_dir_entry_struct* dir_entry); +uint8_t fat16_delete_file(struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry); +uint8_t fat16_create_dir(struct fat16_dir_struct* parent, const char* dir, struct fat16_dir_entry_struct* dir_entry); +#define fat16_delete_dir fat16_delete_file + +void fat16_get_file_modification_date(const struct fat16_dir_entry_struct* dir_entry, uint16_t* year, uint8_t* month, uint8_t* day); +void fat16_get_file_modification_time(const struct fat16_dir_entry_struct* dir_entry, uint8_t* hour, uint8_t* min, uint8_t* sec); + + +uint8_t fat16_get_dir_entry_of_root(struct fat16_fs_struct* fs, struct fat16_dir_entry_struct* dir_entry); + +uint32_t fat16_get_fs_size(const struct fat16_fs_struct* fs); +uint32_t fat16_get_fs_free(const struct fat16_fs_struct* fs); +uint8_t fat16_get_fs_free_callback(uint8_t* buffer, uint32_t offset, void* p); + + +struct fat16_header_struct +{ + uint32_t size; + + uint32_t fat_offset; + uint32_t fat_size; + + uint16_t sector_size; + uint16_t cluster_size; + + uint32_t root_dir_offset; + + uint32_t cluster_zero_offset; +}; + +struct fat16_fs_struct +{ + struct partition_struct* partition; + struct fat16_header_struct header; +}; + +struct fat16_file_struct +{ + struct fat16_fs_struct* fs; + struct fat16_dir_entry_struct dir_entry; + uint32_t pos; + uint16_t pos_cluster; +}; + +struct fat16_dir_struct +{ + struct fat16_fs_struct* fs; + struct fat16_dir_entry_struct dir_entry; + uint16_t entry_next; +}; + +struct fat16_read_callback_arg +{ + uint16_t entry_cur; + uint16_t entry_num; + uint32_t entry_offset; + uint8_t byte_count; +}; + +struct fat16_usage_count_callback_arg +{ + uint16_t cluster_count; + uint8_t buffer_size; +}; + + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16_config.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16_config.h new file mode 100755 index 00000000..c4433981 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/fat16_config.h @@ -0,0 +1,89 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef FAT16_CONFIG_H +#define FAT16_CONFIG_G + +#define UINT16_MAX 65535U +#define nop asm volatile ("nop\n\t") + +/** + * \addtogroup fat16 + * + * @{ + */ +/** + * \file + * FAT16 configuration (license: GPLv2 or LGPLv2.1) + */ + +/** + * \ingroup fat16_config + * Controls FAT16 write support. + * + * Set to 1 to enable FAT16 write support, set to 0 to disable it. + */ +#define FAT16_WRITE_SUPPORT 1 + +/** + * \ingroup fat16_config + * Controls FAT16 date and time support. + * + * Set to 1 to enable FAT16 date and time stamping support. + */ +#define FAT16_DATETIME_SUPPORT 0 + +/** + * \ingroup fat16_config + * Determines the function used for retrieving current date and time. + * + * Define this to the function call which shall be used to retrieve + * current date and time. + * + * \note Used only when FAT16_DATETIME_SUPPORT is 1. + * + * \param[out] year Pointer to a \c uint16_t which receives the current year. + * \param[out] month Pointer to a \c uint8_t which receives the current month. + * \param[out] day Pointer to a \c uint8_t which receives the current day. + * \param[out] hour Pointer to a \c uint8_t which receives the current hour. + * \param[out] min Pointer to a \c uint8_t which receives the current minute. + * \param[out] sec Pointer to a \c uint8_t which receives the current sec. + */ +#if FAT16_DATETIME_SUPPORT +#define fat16_get_datetime(year, month, day, hour, min, sec) \ + get_datetime(year, month, day, hour, min, sec) +/* forward declaration for the above */ +void get_datetime(uint16_t* year, uint8_t* month, uint8_t* day, uint8_t* hour, uint8_t* min, uint8_t* sec); +#endif + +/** + * \ingroup fat16_config + * Maximum number of filesystem handles. + */ +#define FAT16_FS_COUNT 1 + +/** + * \ingroup fat16_config + * Maximum number of file handles. + */ +#define FAT16_FILE_COUNT 1 + +/** + * \ingroup fat16_config + * Maximum number of directory handles. + */ +#define FAT16_DIR_COUNT 1 + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.cpp b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.cpp new file mode 100755 index 00000000..1c379685 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.cpp @@ -0,0 +1,166 @@ +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#include "RepRapSDCard.h" +#include "partition.h" +#include "partition_config.h" +#include "sd-reader_config.h" + +#include <string.h> + +#if USE_DYNAMIC_MEMORY + #include <stdlib.h> +#endif + + +#if !USE_DYNAMIC_MEMORY + extern struct partition_struct partition_handles[]; +#endif + + +/** + * \addtogroup partition Partition table support + * + * Support for reading partition tables and access to partitions. + * + * @{ + */ +/** + * \file + * Partition table implementation (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * \addtogroup partition_config Configuration of partition table support + * Preprocessor defines to configure the partition support. + */ + + +/** + * Opens a partition. + * + * Opens a partition by its index number and returns a partition + * handle which describes the opened partition. + * + * \note This function does not support extended partitions. + * + * \param[in] device_read A function pointer which is used to read from the disk. + * \param[in] device_read_interval A function pointer which is used to read in constant intervals from the disk. + * \param[in] device_write A function pointer which is used to write to the disk. + * \param[in] device_write_interval A function pointer which is used to write a data stream to disk. + * \param[in] index The index of the partition which should be opened, range 0 to 3. + * A negative value is allowed as well. In this case, the partition opened is + * not checked for existance, begins at offset zero, has a length of zero + * and is of an unknown type. + * \returns 0 on failure, a partition descriptor on success. + * \see partition_close + */ +struct partition_struct* partition_open(device_read_t device_read, + device_read_interval_t device_read_interval, + device_write_t device_write, + device_write_interval_t device_write_interval, + int8_t index) +{ + struct partition_struct* new_partition = 0; + uint8_t buffer[0x10]; + + if(index >= 4) + return 0; + + if(index >= 0) + { + /* read specified partition table index */ + if(!device_read(0x01be + index * 0x10, buffer, sizeof(buffer))) + return 0; + + /* abort on empty partition entry */ + if(buffer[4] == 0x00) + return 0; + } + + /* allocate partition descriptor */ +#if USE_DYNAMIC_MEMORY + new_partition = malloc(sizeof(*new_partition)); + if(!new_partition) + return 0; +#else + new_partition = partition_handles; + uint8_t i; + for(i = 0; i < PARTITION_COUNT; ++i) + { + if(new_partition->type == PARTITION_TYPE_FREE) + break; + + ++new_partition; + } + if(i >= PARTITION_COUNT) + return 0; +#endif + + memset(new_partition, 0, sizeof(*new_partition)); + + /* fill partition descriptor */ + new_partition->device_read = device_read; + new_partition->device_read_interval = device_read_interval; + new_partition->device_write = device_write; + new_partition->device_write_interval = device_write_interval; + + if(index >= 0) + { + new_partition->type = buffer[4]; + new_partition->offset = ((uint32_t) buffer[8]) | + ((uint32_t) buffer[9] << 8) | + ((uint32_t) buffer[10] << 16) | + ((uint32_t) buffer[11] << 24); + new_partition->length = ((uint32_t) buffer[12]) | + ((uint32_t) buffer[13] << 8) | + ((uint32_t) buffer[14] << 16) | + ((uint32_t) buffer[15] << 24); + } + else + { + new_partition->type = 0xff; + } + + return new_partition; +} + +/** + * Closes a partition. + * + * This function destroys a partition descriptor which was + * previously obtained from a call to partition_open(). + * When this function returns, the given descriptor will be + * invalid. + * + * \param[in] partition The partition descriptor to destroy. + * \returns 0 on failure, 1 on success. + * \see partition_open + */ +uint8_t partition_close(struct partition_struct* partition) +{ + if(!partition) + return 0; + + /* destroy partition descriptor */ +#if USE_DYNAMIC_MEMORY + free(partition); +#else + partition->type = PARTITION_TYPE_FREE; +#endif + + return 1; +} + +/** + * @} + */ + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.h new file mode 100755 index 00000000..fafb798c --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition.h @@ -0,0 +1,201 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef PARTITION_H +#define PARTITION_H + +#include <stdint.h> + +/** + * \addtogroup partition + * + * @{ + */ +/** + * \file + * Partition table header (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * The partition table entry is not used. + */ +#define PARTITION_TYPE_FREE 0x00 +/** + * The partition contains a FAT12 filesystem. + */ +#define PARTITION_TYPE_FAT12 0x01 +/** + * The partition contains a FAT16 filesystem with 32MB maximum. + */ +#define PARTITION_TYPE_FAT16_32MB 0x04 +/** + * The partition is an extended partition with its own partition table. + */ +#define PARTITION_TYPE_EXTENDED 0x05 +/** + * The partition contains a FAT16 filesystem. + */ +#define PARTITION_TYPE_FAT16 0x06 +/** + * The partition contains a FAT32 filesystem. + */ +#define PARTITION_TYPE_FAT32 0x0b +/** + * The partition contains a FAT32 filesystem with LBA. + */ +#define PARTITION_TYPE_FAT32_LBA 0x0c +/** + * The partition contains a FAT16 filesystem with LBA. + */ +#define PARTITION_TYPE_FAT16_LBA 0x0e +/** + * The partition is an extended partition with LBA. + */ +#define PARTITION_TYPE_EXTENDED_LBA 0x0f +/** + * The partition has an unknown type. + */ +#define PARTITION_TYPE_UNKNOWN 0xff + +/** + * A function pointer used to read from the partition. + * + * \param[in] offset The offset on the device where to start reading. + * \param[out] buffer The buffer into which to place the data. + * \param[in] length The count of bytes to read. + */ +typedef uint8_t (*device_read_t)(uint32_t offset, uint8_t* buffer, uint16_t length); +/** + * A function pointer passed to a \c device_read_interval_t. + * + * \param[in] buffer The buffer which contains the data just read. + * \param[in] offset The offset from which the data in \c buffer was read. + * \param[in] p An opaque pointer. + * \see device_read_interval_t + */ +typedef uint8_t (*device_read_callback_t)(uint8_t* buffer, uint32_t offset, void* p); +/** + * A function pointer used to continuously read units of \c interval bytes + * and call a callback function. + * + * This function starts reading at the specified offset. Every \c interval bytes, + * it calls the callback function with the associated data buffer. + * + * By returning zero, the callback may stop reading. + * + * \param[in] offset Offset from which to start reading. + * \param[in] buffer Pointer to a buffer which is at least interval bytes in size. + * \param[in] interval Number of bytes to read before calling the callback function. + * \param[in] length Number of bytes to read altogether. + * \param[in] callback The function to call every interval bytes. + * \param[in] p An opaque pointer directly passed to the callback function. + * \returns 0 on failure, 1 on success + * \see device_read_t + */ +typedef uint8_t (*device_read_interval_t)(uint32_t offset, uint8_t* buffer, uint16_t interval, uint16_t length, device_read_callback_t callback, void* p); +/** + * A function pointer used to write to the partition. + * + * \param[in] offset The offset on the device where to start writing. + * \param[in] buffer The buffer which to write. + * \param[in] length The count of bytes to write. + */ +typedef uint8_t (*device_write_t)(uint32_t offset, const uint8_t* buffer, uint16_t length); +/** + * A function pointer passed to a \c device_write_interval_t. + * + * \param[in] buffer The buffer which receives the data to write. + * \param[in] offset The offset to which the data in \c buffer will be written. + * \param[in] p An opaque pointer. + * \returns The number of bytes put into \c buffer + * \see device_write_interval_t + */ +typedef uint16_t (*device_write_callback_t)(uint8_t* buffer, uint32_t offset, void* p); +/** + * A function pointer used to continuously write a data stream obtained from + * a callback function. + * + * This function starts writing at the specified offset. To obtain the + * next bytes to write, it calls the callback function. The callback fills the + * provided data buffer and returns the number of bytes it has put into the buffer. + * + * By returning zero, the callback may stop writing. + * + * \param[in] offset Offset where to start writing. + * \param[in] buffer Pointer to a buffer which is used for the callback function. + * \param[in] length Number of bytes to write in total. May be zero for endless writes. + * \param[in] callback The function used to obtain the bytes to write. + * \param[in] p An opaque pointer directly passed to the callback function. + * \returns 0 on failure, 1 on success + * \see device_write_t + */ +typedef uint8_t (*device_write_interval_t)(uint32_t offset, uint8_t* buffer, uint16_t length, device_write_callback_t callback, void* p); + +/** + * Describes a partition. + */ +struct partition_struct +{ + /** + * The function which reads data from the partition. + * + * \note The offset given to this function is relative to the whole disk, + * not to the start of the partition. + */ + device_read_t device_read; + /** + * The function which repeatedly reads a constant amount of data from the partition. + * + * \note The offset given to this function is relative to the whole disk, + * not to the start of the partition. + */ + device_read_interval_t device_read_interval; + /** + * The function which writes data to the partition. + * + * \note The offset given to this function is relative to the whole disk, + * not to the start of the partition. + */ + device_write_t device_write; + /** + * The function which repeatedly writes data to the partition. + * + * \note The offset given to this function is relative to the whole disk, + * not to the start of the partition. + */ + device_write_interval_t device_write_interval; + + /** + * The type of the partition. + * + * Compare this value to the PARTITION_TYPE_* constants. + */ + uint8_t type; + /** + * The offset in bytes on the disk where this partition starts. + */ + uint32_t offset; + /** + * The length in bytes of this partition. + */ + uint32_t length; +}; + +struct partition_struct* partition_open(device_read_t device_read, device_read_interval_t device_read_interval, device_write_t device_write, device_write_interval_t device_write_interval, int8_t index); +uint8_t partition_close(struct partition_struct* partition); + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition_config.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition_config.h new file mode 100755 index 00000000..a750b93c --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/partition_config.h @@ -0,0 +1,34 @@ +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef PARTITION_CONFIG_H +#define PARTITION_CONFIG_G + +/** + * \addtogroup partition + * + * @{ + */ +/** + * \file + * Partition configuration (license: GPLv2 or LGPLv2.1) + */ + +/** + * \ingroup partition_config + * Maximum number of partition handles. + */ +#define PARTITION_COUNT 1 + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd-reader_config.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd-reader_config.h new file mode 100755 index 00000000..33cf2f28 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd-reader_config.h @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef SD_READER_CONFIG_H +#define SD_READER_CONFIG_H + +/** + * \addtogroup config Sd-reader configuration + * + * @{ + */ + +/** + * \file + * Common sd-reader configuration used by all modules (license: GPLv2 or LGPLv2.1) + * + * \note This file contains only configuration items relevant to + * all sd-reader implementation files. For module specific configuration + * options, please see the files fat16_config.h, partition_config.h + * and sd_raw_config.h. + */ + +/** + * Controls allocation of memory. + * + * Set to 1 to use malloc()/free() for allocation of structures + * like file and directory handles, set to 0 to use pre-allocated + * fixed-size handle arrays. + */ +#define USE_DYNAMIC_MEMORY 0 + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.cpp b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.cpp new file mode 100755 index 00000000..7e0e674b --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.cpp @@ -0,0 +1,894 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#include <string.h> +#include <avr/io.h> +#include <HardwareSerial.h> +#include "sd_raw.h" +#include "RepRapSDCard.h" +/** + * \addtogroup sd_raw MMC/SD card raw access + * + * This module implements read and write access to MMC and + * SD cards. It serves as a low-level driver for the higher + * level modules such as partition and file system access. + * + * @{ + */ +/** + * \file + * MMC/SD raw access implementation (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * \addtogroup sd_raw_config MMC/SD configuration + * Preprocessor defines to configure the MMC/SD support. + */ + +/** + * @} + */ + +/* commands available in SPI mode */ + +/* CMD0: response R1 */ +#define CMD_GO_IDLE_STATE 0x00 +/* CMD1: response R1 */ +#define CMD_SEND_OP_COND 0x01 +/* CMD9: response R1 */ +#define CMD_SEND_CSD 0x09 +/* CMD10: response R1 */ +#define CMD_SEND_CID 0x0a +/* CMD12: response R1 */ +#define CMD_STOP_TRANSMISSION 0x0c +/* CMD13: response R2 */ +#define CMD_SEND_STATUS 0x0d +/* CMD16: arg0[31:0]: block length, response R1 */ +#define CMD_SET_BLOCKLEN 0x10 +/* CMD17: arg0[31:0]: data address, response R1 */ +#define CMD_READ_SINGLE_BLOCK 0x11 +/* CMD18: arg0[31:0]: data address, response R1 */ +#define CMD_READ_MULTIPLE_BLOCK 0x12 +/* CMD24: arg0[31:0]: data address, response R1 */ +#define CMD_WRITE_SINGLE_BLOCK 0x18 +/* CMD25: arg0[31:0]: data address, response R1 */ +#define CMD_WRITE_MULTIPLE_BLOCK 0x19 +/* CMD27: response R1 */ +#define CMD_PROGRAM_CSD 0x1b +/* CMD28: arg0[31:0]: data address, response R1b */ +#define CMD_SET_WRITE_PROT 0x1c +/* CMD29: arg0[31:0]: data address, response R1b */ +#define CMD_CLR_WRITE_PROT 0x1d +/* CMD30: arg0[31:0]: write protect data address, response R1 */ +#define CMD_SEND_WRITE_PROT 0x1e +/* CMD32: arg0[31:0]: data address, response R1 */ +#define CMD_TAG_SECTOR_START 0x20 +/* CMD33: arg0[31:0]: data address, response R1 */ +#define CMD_TAG_SECTOR_END 0x21 +/* CMD34: arg0[31:0]: data address, response R1 */ +#define CMD_UNTAG_SECTOR 0x22 +/* CMD35: arg0[31:0]: data address, response R1 */ +#define CMD_TAG_ERASE_GROUP_START 0x23 +/* CMD36: arg0[31:0]: data address, response R1 */ +#define CMD_TAG_ERASE_GROUP_END 0x24 +/* CMD37: arg0[31:0]: data address, response R1 */ +#define CMD_UNTAG_ERASE_GROUP 0x25 +/* CMD38: arg0[31:0]: stuff bits, response R1b */ +#define CMD_ERASE 0x26 +/* CMD42: arg0[31:0]: stuff bits, response R1b */ +#define CMD_LOCK_UNLOCK 0x2a +/* CMD58: response R3 */ +#define CMD_READ_OCR 0x3a +/* CMD59: arg0[31:1]: stuff bits, arg0[0:0]: crc option, response R1 */ +#define CMD_CRC_ON_OFF 0x3b + +/* command responses */ +/* R1: size 1 byte */ +#define R1_IDLE_STATE 0 +#define R1_ERASE_RESET 1 +#define R1_ILL_COMMAND 2 +#define R1_COM_CRC_ERR 3 +#define R1_ERASE_SEQ_ERR 4 +#define R1_ADDR_ERR 5 +#define R1_PARAM_ERR 6 +/* R1b: equals R1, additional busy bytes */ +/* R2: size 2 bytes */ +#define R2_CARD_LOCKED 0 +#define R2_WP_ERASE_SKIP 1 +#define R2_ERR 2 +#define R2_CARD_ERR 3 +#define R2_CARD_ECC_FAIL 4 +#define R2_WP_VIOLATION 5 +#define R2_INVAL_ERASE 6 +#define R2_OUT_OF_RANGE 7 +#define R2_CSD_OVERWRITE 7 +#define R2_IDLE_STATE (R1_IDLE_STATE + 8) +#define R2_ERASE_RESET (R1_ERASE_RESET + 8) +#define R2_ILL_COMMAND (R1_ILL_COMMAND + 8) +#define R2_COM_CRC_ERR (R1_COM_CRC_ERR + 8) +#define R2_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 8) +#define R2_ADDR_ERR (R1_ADDR_ERR + 8) +#define R2_PARAM_ERR (R1_PARAM_ERR + 8) +/* R3: size 5 bytes */ +#define R3_OCR_MASK (0xffffffffUL) +#define R3_IDLE_STATE (R1_IDLE_STATE + 32) +#define R3_ERASE_RESET (R1_ERASE_RESET + 32) +#define R3_ILL_COMMAND (R1_ILL_COMMAND + 32) +#define R3_COM_CRC_ERR (R1_COM_CRC_ERR + 32) +#define R3_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 32) +#define R3_ADDR_ERR (R1_ADDR_ERR + 32) +#define R3_PARAM_ERR (R1_PARAM_ERR + 32) +/* Data Response: size 1 byte */ +#define DR_STATUS_MASK 0x0e +#define DR_STATUS_ACCEPTED 0x05 +#define DR_STATUS_CRC_ERR 0x0a +#define DR_STATUS_WRITE_ERR 0x0c + + + + + /* static data buffer for acceleration */ + uint8_t raw_block[512]; + /* offset where the data within raw_block lies on the card */ + uint32_t raw_block_address; +#if SD_RAW_WRITE_BUFFERING + /* flag to remember if raw_block was written to the card */ + uint8_t raw_block_written; +#endif + +/* private helper functions */ +static void sd_raw_send_byte(uint8_t b); +static uint8_t sd_raw_rec_byte(); +static uint8_t sd_raw_send_command_r1(uint8_t command, uint32_t arg); + +/** + * \ingroup sd_raw + * Initializes memory card communication. + * + * \returns 0 on failure, 1 on success. + */ +uint8_t sd_raw_init() +{ + uint16_t i; + uint8_t response; + + /* enable inputs for reading card status */ + configure_pin_available(); + configure_pin_locked(); + + /* enable outputs for MOSI, SCK, SS, input for MISO */ + configure_pin_mosi(); + configure_pin_sck(); + configure_pin_ss(); + configure_pin_miso(); + + unselect_card(); + + /* initialize SPI with lowest frequency; max. 400kHz during identification mode of card */ + SPCR = (0 << SPIE) | /* SPI Interrupt Enable */ + (1 << SPE) | /* SPI Enable */ + (0 << DORD) | /* Data Order: MSB first */ + (1 << MSTR) | /* Master mode */ + (0 << CPOL) | /* Clock Polarity: SCK low when idle */ + (0 << CPHA) | /* Clock Phase: sample on rising SCK edge */ + (1 << SPR1) | /* Clock Frequency: f_OSC / 128 */ + (1 << SPR0); + SPSR &= ~(1 << SPI2X); /* No doubled clock frequency */ + + /* initialization procedure */ + + if(!sd_raw_available()) + return 0; + + /* card needs 74 cycles minimum to start up */ + for(i = 0; i < 10; ++i) + { + /* wait 8 clock cycles */ + sd_raw_rec_byte(); + } + + /* address card */ + select_card(); + + /* reset card */ + for(i = 0; ; ++i) + { + response = sd_raw_send_command_r1(CMD_GO_IDLE_STATE, 0); + if(response == (1 << R1_IDLE_STATE)) + break; + + if(i == 0x1ff) + { + unselect_card(); + return 0; + } + } + + /* wait for card to get ready */ + for(i = 0; ; ++i) + { + response = sd_raw_send_command_r1(CMD_SEND_OP_COND, 0); + if(!(response & (1 << R1_IDLE_STATE))) + break; + + if(i == 0x7fff) + { + unselect_card(); + return 0; + } + } + + /* set block size to 512 bytes */ + if(sd_raw_send_command_r1(CMD_SET_BLOCKLEN, 512)) + { + unselect_card(); + return 0; + } + + /* deaddress card */ + unselect_card(); + + /* switch to highest SPI frequency possible */ + SPCR &= ~((1 << SPR1) | (1 << SPR0)); /* Clock Frequency: f_OSC / 4 */ + SPSR |= (1 << SPI2X); /* Doubled Clock Frequency: f_OSC / 2 */ + +#if !SD_RAW_SAVE_RAM + /* the first block is likely to be accessed first, so precache it here */ + raw_block_address = 0xffffffff; +#if SD_RAW_WRITE_BUFFERING + raw_block_written = 1; +#endif + if(!sd_raw_read(0, raw_block, sizeof(raw_block))) + return 0; +#endif + + return 1; +} + +/** + * \ingroup sd_raw + * Checks wether a memory card is located in the slot. + * + * \returns 1 if the card is available, 0 if it is not. + */ +uint8_t sd_raw_available() +{ + return get_pin_available() == 0x00; +} + +/** + * \ingroup sd_raw + * Checks wether the memory card is locked for write access. + * + * \returns 1 if the card is locked, 0 if it is not. + */ +uint8_t sd_raw_locked() +{ + return get_pin_locked() == 0x00; +} + +/** + * \ingroup sd_raw + * Sends a raw byte to the memory card. + * + * \param[in] b The byte to sent. + * \see sd_raw_rec_byte + */ +void sd_raw_send_byte(uint8_t b) +{ + SPDR = b; + /* wait for byte to be shifted out */ + while(!(SPSR & (1 << SPIF))); + SPSR &= ~(1 << SPIF); +} + +/** + * \ingroup sd_raw + * Receives a raw byte from the memory card. + * + * \returns The byte which should be read. + * \see sd_raw_send_byte + */ +uint8_t sd_raw_rec_byte() +{ + /* send dummy data for receiving some */ + SPDR = 0xff; + while(!(SPSR & (1 << SPIF))); + SPSR &= ~(1 << SPIF); + + return SPDR; +} + +/** + * \ingroup sd_raw + * Send a command to the memory card which responses with a R1 response. + * + * \param[in] command The command to send. + * \param[in] arg The argument for command. + * \returns The command answer. + */ +uint8_t sd_raw_send_command_r1(uint8_t command, uint32_t arg) +{ + uint8_t response; + uint8_t i; + + /* wait some clock cycles */ + sd_raw_rec_byte(); + + /* send command via SPI */ + sd_raw_send_byte(0x40 | command); + sd_raw_send_byte((arg >> 24) & 0xff); + sd_raw_send_byte((arg >> 16) & 0xff); + sd_raw_send_byte((arg >> 8) & 0xff); + sd_raw_send_byte((arg >> 0) & 0xff); + sd_raw_send_byte(command == CMD_GO_IDLE_STATE ? 0x95 : 0xff); + + /* receive response */ + for(i = 0; i < 10; ++i) + { + response = sd_raw_rec_byte(); + if(response != 0xff) + break; + } + + return response; +} + +/** + * \ingroup sd_raw + * Reads raw data from the card. + * + * \param[in] offset The offset from which to read. + * \param[out] buffer The buffer into which to write the data. + * \param[in] length The number of bytes to read. + * \returns 0 on failure, 1 on success. + * \see sd_raw_read_interval, sd_raw_write, sd_raw_write_interval + */ +uint8_t sd_raw_read(uint32_t offset, uint8_t* buffer, uint16_t length) +{ + uint32_t block_address; + uint16_t block_offset; + uint16_t read_length; + uint16_t i; + + while(length > 0) + { + /* determine byte count to read at once */ + block_address = offset & 0xfffffe00; + block_offset = offset & 0x01ff; + read_length = 512 - block_offset; /* read up to block border */ + if(read_length > length) + read_length = length; + +#if !SD_RAW_SAVE_RAM + /* check if the requested data is cached */ + if(block_address != raw_block_address) +#endif + { +#if SD_RAW_WRITE_BUFFERING + if(!raw_block_written) + { + if(!sd_raw_write(raw_block_address, raw_block, sizeof(raw_block))) + return 0; + } +#endif + /* address card */ + select_card(); + + /* send single block request */ + if(sd_raw_send_command_r1(CMD_READ_SINGLE_BLOCK, block_address)) + { + unselect_card(); + return 0; + } + + /* wait for data block (start byte 0xfe) */ + + i = 0; + while(sd_raw_rec_byte() != 0xfe) { + i++; + if (i == 0) { + unselect_card(); + return 0; + } + } + +#if SD_RAW_SAVE_RAM + /* read byte block */ + uint16_t read_to = block_offset + read_length; + for(i = 0; i < 512; ++i) + { + uint8_t b = sd_raw_rec_byte(); + if(i >= block_offset && i < read_to) + *buffer++ = b; + } +#else + /* read byte block */ + uint8_t* cache = raw_block; + for(i = 0; i < 512; ++i) + *cache++ = sd_raw_rec_byte(); + raw_block_address = block_address; + + memcpy(buffer, raw_block + block_offset, read_length); + buffer += read_length; +#endif + + /* read crc16 */ + sd_raw_rec_byte(); + sd_raw_rec_byte(); + + /* deaddress card */ + unselect_card(); + + /* let card some time to finish */ + sd_raw_rec_byte(); + } +#if !SD_RAW_SAVE_RAM + else + { + /* use cached data */ + memcpy(buffer, raw_block + block_offset, read_length); + buffer += read_length; + } +#endif + + length -= read_length; + offset += read_length; + } + + return 1; +} + +/** + * \ingroup sd_raw + * Continuously reads units of \c interval bytes and calls a callback function. + * + * This function starts reading at the specified offset. Every \c interval bytes, + * it calls the callback function with the associated data buffer. + * + * By returning zero, the callback may stop reading. + * + * \note Within the callback function, you can not start another read or + * write operation. + * \note This function only works if the following conditions are met: + * - (offset - (offset % 512)) % interval == 0 + * - length % interval == 0 + * + * \param[in] offset Offset from which to start reading. + * \param[in] buffer Pointer to a buffer which is at least interval bytes in size. + * \param[in] interval Number of bytes to read before calling the callback function. + * \param[in] length Number of bytes to read altogether. + * \param[in] callback The function to call every interval bytes. + * \param[in] p An opaque pointer directly passed to the callback function. + * \returns 0 on failure, 1 on success + * \see sd_raw_write_interval, sd_raw_read, sd_raw_write + */ +uint8_t sd_raw_read_interval(uint32_t offset, uint8_t* buffer, uint16_t interval, uint16_t length, sd_raw_read_interval_handler_t callback, void* p) +{ + if(!buffer || interval == 0 || length < interval || !callback) + return 0; + +#if !SD_RAW_SAVE_RAM + while(length >= interval) + { + /* as reading is now buffered, we directly + * hand over the request to sd_raw_read() + */ + if(!sd_raw_read(offset, buffer, interval)) + return 0; + if(!callback(buffer, offset, p)) + break; + offset += interval; + length -= interval; + } + + return 1; +#else + /* address card */ + select_card(); + + uint16_t block_offset; + uint16_t read_length; + uint8_t* buffer_cur; + uint8_t finished = 0; + do + { + /* determine byte count to read at once */ + block_offset = offset & 0x01ff; + read_length = 512 - block_offset; + + /* send single block request */ + if(sd_raw_send_command_r1(CMD_READ_SINGLE_BLOCK, offset & 0xfffffe00)) + { + unselect_card(); + return 0; + } + + /* wait for data block (start byte 0xfe) */ + while(sd_raw_rec_byte() != 0xfe); + + /* read up to the data of interest */ + for(i = 0; i < block_offset; ++i) + sd_raw_rec_byte(); + + /* read interval bytes of data and execute the callback */ + do + { + if(read_length < interval || length < interval) + break; + + buffer_cur = buffer; + for(i = 0; i < interval; ++i) + *buffer_cur++ = sd_raw_rec_byte(); + + if(!callback(buffer, offset + (512 - read_length), p)) + { + finished = 1; + break; + } + + read_length -= interval; + length -= interval; + + } while(read_length > 0 && length > 0); + + /* read rest of data block */ + while(read_length-- > 0) + sd_raw_rec_byte(); + + /* read crc16 */ + sd_raw_rec_byte(); + sd_raw_rec_byte(); + + if(length < interval) + break; + + offset = (offset & 0xfffffe00) + 512; + + } while(!finished); + + /* deaddress card */ + unselect_card(); + + /* let card some time to finish */ + sd_raw_rec_byte(); + + return 1; +#endif +} + +/** + * \ingroup sd_raw + * Writes raw data to the card. + * + * \note If write buffering is enabled, you might have to + * call sd_raw_sync() before disconnecting the card + * to ensure all remaining data has been written. + * + * \param[in] offset The offset where to start writing. + * \param[in] buffer The buffer containing the data to be written. + * \param[in] length The number of bytes to write. + * \returns 0 on failure, 1 on success. + * \see sd_raw_write_interval, sd_raw_read, sd_raw_read_interval + */ +uint8_t sd_raw_write(uint32_t offset, const uint8_t* buffer, uint16_t length) +{ +#if SD_RAW_WRITE_SUPPORT + + if(get_pin_locked()) + { + //Serial.println("locked."); + return 0; + } + + uint32_t block_address; + uint16_t block_offset; + uint16_t write_length; + uint16_t i; + + while(length > 0) + { + /* determine byte count to write at once */ + block_address = offset & 0xfffffe00; + block_offset = offset & 0x01ff; + write_length = 512 - block_offset; /* write up to block border */ + if(write_length > length) + write_length = length; + + /* Merge the data to write with the content of the block. + * Use the cached block if available. + */ + if(block_address != raw_block_address) + { +#if SD_RAW_WRITE_BUFFERING + if(!raw_block_written) + { + if(!sd_raw_write(raw_block_address, raw_block, sizeof(raw_block))) + return 0; + } +#endif + + if(block_offset || write_length < 512) + { + if(!sd_raw_read(block_address, raw_block, sizeof(raw_block))) + return 0; + } + raw_block_address = block_address; + } + + if(buffer != raw_block) + { + memcpy(raw_block + block_offset, buffer, write_length); + +#if SD_RAW_WRITE_BUFFERING + raw_block_written = 0; + + if(length == write_length) + return 1; +#endif + } + + buffer += write_length; + + /* address card */ + select_card(); + + /* send single block request */ + if(sd_raw_send_command_r1(CMD_WRITE_SINGLE_BLOCK, block_address)) + { + unselect_card(); + return 0; + } + + /* send start byte */ + sd_raw_send_byte(0xfe); + + /* write byte block */ + uint8_t* cache = raw_block; + for(i = 0; i < 512; ++i) + sd_raw_send_byte(*cache++); + + /* write dummy crc16 */ + sd_raw_send_byte(0xff); + sd_raw_send_byte(0xff); + + /* wait while card is busy */ + while(sd_raw_rec_byte() != 0xff); + sd_raw_rec_byte(); + + /* deaddress card */ + unselect_card(); + + length -= write_length; + offset += write_length; + +#if SD_RAW_WRITE_BUFFERING + raw_block_written = 1; +#endif + } + + return 1; +#else + return 0; +#endif +} + +/** + * \ingroup sd_raw + * Writes a continuous data stream obtained from a callback function. + * + * This function starts writing at the specified offset. To obtain the + * next bytes to write, it calls the callback function. The callback fills the + * provided data buffer and returns the number of bytes it has put into the buffer. + * + * By returning zero, the callback may stop writing. + * + * \param[in] offset Offset where to start writing. + * \param[in] buffer Pointer to a buffer which is used for the callback function. + * \param[in] length Number of bytes to write in total. May be zero for endless writes. + * \param[in] callback The function used to obtain the bytes to write. + * \param[in] p An opaque pointer directly passed to the callback function. + * \returns 0 on failure, 1 on success + * \see sd_raw_read_interval, sd_raw_write, sd_raw_read + */ +uint8_t sd_raw_write_interval(uint32_t offset, uint8_t* buffer, uint16_t length, sd_raw_write_interval_handler_t callback, void* p) +{ +#if SD_RAW_WRITE_SUPPORT + +#if SD_RAW_SAVE_RAM + #error "SD_RAW_WRITE_SUPPORT is not supported together with SD_RAW_SAVE_RAM" +#endif + + if(!buffer || !callback) + return 0; + + uint8_t endless = (length == 0); + while(endless || length > 0) + { + uint16_t bytes_to_write = callback(buffer, offset, p); + if(!bytes_to_write) + break; + if(!endless && bytes_to_write > length) + return 0; + + /* as writing is always buffered, we directly + * hand over the request to sd_raw_write() + */ + if(!sd_raw_write(offset, buffer, bytes_to_write)) + return 0; + + offset += bytes_to_write; + length -= bytes_to_write; + } + + return 1; + +#else + return 0; +#endif +} + +/** + * \ingroup sd_raw + * Writes the write buffer's content to the card. + * + * \note When write buffering is enabled, you should + * call this function before disconnecting the + * card to ensure all remaining data has been + * written. + * + * \returns 0 on failure, 1 on success. + * \see sd_raw_write + */ +uint8_t sd_raw_sync() +{ +#if SD_RAW_WRITE_SUPPORT +#if SD_RAW_WRITE_BUFFERING + if(raw_block_written) + return 1; + if(!sd_raw_write(raw_block_address, raw_block, sizeof(raw_block))) + return 0; +#endif + return 1; +#else + return 0; +#endif +} + +/** + * \ingroup sd_raw + * Reads informational data from the card. + * + * This function reads and returns the card's registers + * containing manufacturing and status information. + * + * \note: The information retrieved by this function is + * not required in any way to operate on the card, + * but it might be nice to display some of the data + * to the user. + * + * \param[in] info A pointer to the structure into which to save the information. + * \returns 0 on failure, 1 on success. + */ +uint8_t sd_raw_get_info(struct sd_raw_info* info) +{ + uint8_t i; + + if(!info || !sd_raw_available()) + return 0; + + memset(info, 0, sizeof(*info)); + + select_card(); + + /* read cid register */ + if(sd_raw_send_command_r1(CMD_SEND_CID, 0)) + { + unselect_card(); + return 0; + } + while(sd_raw_rec_byte() != 0xfe); + for(i = 0; i < 18; ++i) + { + uint8_t b = sd_raw_rec_byte(); + + switch(i) + { + case 0: + info->manufacturer = b; + break; + case 1: + case 2: + info->oem[i - 1] = b; + break; + case 3: + case 4: + case 5: + case 6: + case 7: + info->product[i - 3] = b; + break; + case 8: + info->revision = b; + break; + case 9: + case 10: + case 11: + case 12: + info->serial |= (uint32_t) b << ((12 - i) * 8); + break; + case 13: + info->manufacturing_year = b << 4; + break; + case 14: + info->manufacturing_year |= b >> 4; + info->manufacturing_month = b & 0x0f; + break; + } + } + + /* read csd register */ + uint8_t csd_read_bl_len = 0; + uint8_t csd_c_size_mult = 0; + uint16_t csd_c_size = 0; + if(sd_raw_send_command_r1(CMD_SEND_CSD, 0)) + { + unselect_card(); + return 0; + } + while(sd_raw_rec_byte() != 0xfe); + for(i = 0; i < 18; ++i) + { + uint8_t b = sd_raw_rec_byte(); + + switch(i) + { + case 5: + csd_read_bl_len = b & 0x0f; + break; + case 6: + csd_c_size = (uint16_t) (b & 0x03) << 8; + break; + case 7: + csd_c_size |= b; + csd_c_size <<= 2; + break; + case 8: + csd_c_size |= b >> 6; + ++csd_c_size; + break; + case 9: + csd_c_size_mult = (b & 0x03) << 1; + break; + case 10: + csd_c_size_mult |= b >> 7; + + info->capacity = (uint32_t) csd_c_size << (csd_c_size_mult + csd_read_bl_len + 2); + + break; + case 14: + if(b & 0x40) + info->flag_copy = 1; + if(b & 0x20) + info->flag_write_protect = 1; + if(b & 0x10) + info->flag_write_protect_temp = 1; + info->format = (b & 0x0c) >> 2; + break; + } + } + + unselect_card(); + + return 1; +} + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.h new file mode 100755 index 00000000..ade23935 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw.h @@ -0,0 +1,139 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef SD_RAW_H +#define SD_RAW_H + +#include <stdint.h> +#include "sd_raw_config.h" + +/** + * \addtogroup sd_raw + * + * @{ + */ +/** + * \file + * MMC/SD raw access header (license: GPLv2 or LGPLv2.1) + * + * \author Roland Riegel + */ + +/** + * The card's layout is harddisk-like, which means it contains + * a master boot record with a partition table. + */ +#define SD_RAW_FORMAT_HARDDISK 0 +/** + * The card contains a single filesystem and no partition table. + */ +#define SD_RAW_FORMAT_SUPERFLOPPY 1 +/** + * The card's layout follows the Universal File Format. + */ +#define SD_RAW_FORMAT_UNIVERSAL 2 +/** + * The card's layout is unknown. + */ +#define SD_RAW_FORMAT_UNKNOWN 3 + +/** + * This struct is used by sd_raw_get_info() to return + * manufacturing and status information of the card. + */ +struct sd_raw_info +{ + /** + * A manufacturer code globally assigned by the SD card organization. + */ + uint8_t manufacturer; + /** + * A string describing the card's OEM or content, globally assigned by the SD card organization. + */ + uint8_t oem[3]; + /** + * A product name. + */ + uint8_t product[6]; + /** + * The card's revision, coded in packed BCD. + * + * For example, the revision value \c 0x32 means "3.2". + */ + uint8_t revision; + /** + * A serial number assigned by the manufacturer. + */ + uint32_t serial; + /** + * The year of manufacturing. + * + * A value of zero means year 2000. + */ + uint8_t manufacturing_year; + /** + * The month of manufacturing. + */ + uint8_t manufacturing_month; + /** + * The card's total capacity in bytes. + */ + uint32_t capacity; + /** + * Defines wether the card's content is original or copied. + * + * A value of \c 0 means original, \c 1 means copied. + */ + uint8_t flag_copy; + /** + * Defines wether the card's content is write-protected. + * + * \note This is an internal flag and does not represent the + * state of the card's mechanical write-protect switch. + */ + uint8_t flag_write_protect; + /** + * Defines wether the card's content is temporarily write-protected. + * + * \note This is an internal flag and does not represent the + * state of the card's mechanical write-protect switch. + */ + uint8_t flag_write_protect_temp; + /** + * The card's data layout. + * + * See the \c SD_RAW_FORMAT_* constants for details. + * + * \note This value is not guaranteed to match reality. + */ + uint8_t format; +}; + +typedef uint8_t (*sd_raw_read_interval_handler_t)(uint8_t* buffer, uint32_t offset, void* p); +typedef uint16_t (*sd_raw_write_interval_handler_t)(uint8_t* buffer, uint32_t offset, void* p); + +uint8_t sd_raw_init(); +uint8_t sd_raw_available(); +uint8_t sd_raw_locked(); + +uint8_t sd_raw_read(uint32_t offset, uint8_t* buffer, uint16_t length); +uint8_t sd_raw_read_interval(uint32_t offset, uint8_t* buffer, uint16_t interval, uint16_t length, sd_raw_read_interval_handler_t callback, void* p); +uint8_t sd_raw_write(uint32_t offset, const uint8_t* buffer, uint16_t length); +uint8_t sd_raw_write_interval(uint32_t offset, uint8_t* buffer, uint16_t length, sd_raw_write_interval_handler_t callback, void* p); +uint8_t sd_raw_sync(); + +uint8_t sd_raw_get_info(struct sd_raw_info* info); + +/** + * @} + */ + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw_config.h b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw_config.h new file mode 100755 index 00000000..c5f0f8c9 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/RepRapSDCard/sd_raw_config.h @@ -0,0 +1,112 @@ + +/* + * Copyright (c) 2006-2007 by Roland Riegel <feedback@roland-riegel.de> + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef SD_RAW_CONFIG_H +#define SD_RAW_CONFIG_H + +/** + * \addtogroup sd_raw + * + * @{ + */ +/** + * \file + * MMC/SD support configuration (license: GPLv2 or LGPLv2.1) + */ + +/** + * \ingroup sd_raw_config + * Controls MMC/SD write support. + * + * Set to 1 to enable MMC/SD write support, set to 0 to disable it. + */ +#define SD_RAW_WRITE_SUPPORT 1 + +/** + * \ingroup sd_raw_config + * Controls MMC/SD write buffering. + * + * Set to 1 to buffer write accesses, set to 0 to disable it. + * + * \note This option has no effect when SD_RAW_WRITE_SUPPORT is 0. + */ +#define SD_RAW_WRITE_BUFFERING 1 + +/** + * \ingroup sd_raw_config + * Controls MMC/SD access buffering. + * + * Set to 1 to save static RAM, but be aware that you will + * lose performance. + * + * \note When SD_RAW_WRITE_SUPPORT is 1, SD_RAW_SAVE_RAM will + * be reset to 0. + */ +#define SD_RAW_SAVE_RAM 1 + +/* defines for customisation of sd/mmc port access */ +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) + #define configure_pin_mosi() DDRB |= (1 << DDB3) + #define configure_pin_sck() DDRB |= (1 << DDB5) + #define configure_pin_ss() DDRB |= (1 << DDB2) + #define configure_pin_miso() DDRB &= ~(1 << DDB4) + + #define select_card() PORTB &= ~(1 << PB2) + #define unselect_card() PORTB |= (1 << PB2) +#elif defined(__AVR_ATmega16__) || \ + defined(__AVR_ATmega32__) || \ + defined(__AVR_ATmega644__) || \ + defined(__AVR_ATmega644P__) + #define configure_pin_mosi() DDRB |= (1 << DDB5) + #define configure_pin_sck() DDRB |= (1 << DDB7) + #define configure_pin_ss() DDRB |= (1 << DDB4) + #define configure_pin_miso() DDRB &= ~(1 << DDB6) + + #define select_card() PORTB &= ~(1 << PB4) + #define unselect_card() PORTB |= (1 << PB4) +#elif defined(__AVR_ATmega64__) || \ + defined(__AVR_ATmega128__) || \ + defined(__AVR_ATmega169__) + #define configure_pin_mosi() DDRB |= (1 << DDB2) + #define configure_pin_sck() DDRB |= (1 << DDB1) + #define configure_pin_ss() DDRB |= (1 << DDB0) + #define configure_pin_miso() DDRB &= ~(1 << DDB3) + + //TODO: update with real values. + #define select_card() PORTB &= ~(1 << PB0) + #define unselect_card() PORTB |= (1 << PB0) +#else + #error "no sd/mmc pin mapping available!" +#endif + +#define configure_pin_available() DDRA &= ~(1 << DDA7) +#define configure_pin_locked() DDRA &= ~(1 << DDA3) + +#define get_pin_available() ((PINA >> PC7) & 0x01) +#define get_pin_locked() ((PINA >> PC3) & 0x01) + +/** + * @} + */ + +/* configuration checks */ +#if SD_RAW_WRITE_SUPPORT +#undef SD_RAW_SAVE_RAM +#define SD_RAW_SAVE_RAM 0 +#else +#undef SD_RAW_WRITE_BUFFERING +#define SD_RAW_WRITE_BUFFERING 0 +#endif + +#endif + diff --git a/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.cpp b/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.cpp new file mode 100644 index 00000000..216b8291 --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.cpp @@ -0,0 +1,198 @@ +#include <SimplePacket.h> +#include <avr/io.h> + +SimplePacket::SimplePacket(txFuncPtr myPtr) +{ + txFunc = myPtr; + init(); +} + +void SimplePacket::init() +{ + //zero out our data arrays. + for (rx_length = 0; rx_length < MAX_PACKET_LENGTH; rx_length++) + rx_data[rx_length] = 0; + for (tx_length = 0; tx_length < MAX_PACKET_LENGTH; tx_length++) + tx_data[tx_length] = 0; + + //init our other variables. + state = PS_START; + response_code = RC_OK; + target_length = 0; + rx_length = 0; + rx_crc = 0; + tx_length = 0; + tx_crc = 0; +} + +//process a byte from our packet +void SimplePacket::process_byte(uint8_t b) +{ + if (state == PS_START) // process start byte + { + //cool! its the start of a packet. + if (b == START_BYTE) + { + init(); + state = PS_LEN; + } + else + { + // throw an error message? + // nah, ignore it as garbage. + // FIXME: Consider reporting such error in a special debug mode + } + } + else if (state == PS_LEN) // process length byte + { + target_length = b; + rx_length = 0; + state = PS_PAYLOAD; + + if (target_length > MAX_PACKET_LENGTH) + response_code = RC_PACKET_TOO_BIG; + } + else if (state == PS_PAYLOAD) // process payload byte + { + //just keep reading bytes while we got them. + if (rx_length < target_length) + { + //keep track of CRC. + rx_crc = _crc_ibutton_update(rx_crc, b); + + //will it fit? + if (rx_length < MAX_PACKET_LENGTH) + rx_data[rx_length] = b; + + //keep track. + rx_length++; + } + + //are we done? + if (rx_length >= target_length) + state = PS_CRC; + } + else if (state == PS_CRC) // check crc + { + // did the packet check out? + if (rx_crc != b) + response_code = RC_CRC_MISMATCH; + + //okay, the packet is done. + state = PS_LAST; + } +} + +bool SimplePacket::isFinished() +{ + return (state == PS_LAST); +} + +uint8_t SimplePacket::getLength() +{ + return rx_length; +} + +PacketState SimplePacket::getState() +{ + return state; +} + +ResponseCode SimplePacket::getResponseCode() +{ + return response_code; +} + +void SimplePacket::unsupported() +{ + response_code = RC_CMD_UNSUPPORTED; +} + +void SimplePacket::overflow() +{ + response_code = RC_BUFFER_OVERFLOW; +} + +void SimplePacket::sendReply() +{ + //initialize our response CRC + tx_crc = 0; + tx_crc = _crc_ibutton_update(tx_crc, response_code); + + //actually send our response. + transmit(START_BYTE); + transmit(tx_length+1); + transmit(response_code); + + //loop through our reply packet payload and send it. + for (uint8_t i=0; i<tx_length; i++) + { + transmit(tx_data[i]); + tx_crc = _crc_ibutton_update(tx_crc, tx_data[i]); + } + + //okay, send our CRC. + transmit(tx_crc); + + //okay, now reset. + init(); +} + +void SimplePacket::sendPacket() +{ + tx_crc = 0; + transmit(START_BYTE); + transmit(tx_length); + + //loop through our reply packet payload and send it. + for (uint8_t i=0; i<tx_length; i++) + { + transmit(tx_data[i]); + tx_crc = _crc_ibutton_update(tx_crc, tx_data[i]); + } + + //okay, send our CRC. + transmit(tx_crc); +} + +void SimplePacket::transmit(uint8_t d) +{ + txFunc(d); +} + +//add a four byte chunk of data to our reply +void SimplePacket::add_32(uint32_t d) +{ + add_16(d); + add_16(d >> 16); +} + +//add a two byte chunk of data to our reply +void SimplePacket::add_16(uint16_t d) +{ + add_8(d & 0xff); + add_8(d >> 8); +} + +//add a byte to our reply. +void SimplePacket::add_8(uint8_t d) +{ + //only add it if it will fit. + if (tx_length < MAX_PACKET_LENGTH) + tx_data[tx_length++] = d; +} + +uint8_t SimplePacket::get_8(uint8_t idx) +{ + return rx_data[idx]; +} + +uint16_t SimplePacket::get_16(uint8_t idx) +{ + return (((int16_t)get_8(idx+1)) << 8) | get_8(idx); +} + +uint32_t SimplePacket::get_32(uint8_t idx) +{ + return (((uint32_t)get_16(idx+2)) << 16) | get_16(idx); +} diff --git a/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.h b/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.h new file mode 100644 index 00000000..cd53140c --- /dev/null +++ b/tags/firmware/gen3/1.1/libraries/SimplePacket/SimplePacket.h @@ -0,0 +1,78 @@ +#ifndef _SIMPLE_PACKET_H_ +#define _SIMPLE_PACKET_H_ + +//include our various libraries. +#include <util/crc16.h> +#include <stdint.h> +#include "HardwareSerial.h" + +#define START_BYTE 0xD5 +#define MAX_PACKET_LENGTH 32 + +typedef void (*txFuncPtr)(uint8_t); + +// packet states +typedef enum { + PS_START = 0, + PS_LEN, + PS_PAYLOAD, + PS_CRC, + PS_LAST +} +PacketState; + +// various error codes +typedef enum { + RC_GENERIC_ERROR = 0, + RC_OK = 1, + RC_BUFFER_OVERFLOW = 2, + RC_CRC_MISMATCH = 3, + RC_PACKET_TOO_BIG = 4, + RC_CMD_UNSUPPORTED = 5 +} +ResponseCode; + +class SimplePacket { +private: + //variables for our incoming packet. + PacketState state; + uint8_t target_length; + uint8_t rx_length; + uint8_t rx_data[MAX_PACKET_LENGTH]; + uint8_t rx_crc; + uint8_t tx_length; + uint8_t tx_data[MAX_PACKET_LENGTH]; + uint8_t tx_crc; + ResponseCode response_code; + + txFuncPtr txFunc; + +public: + + SimplePacket(txFuncPtr myPtr); + void init(); + + //process a byte from our packet + void process_byte(uint8_t b); + bool isFinished(); + uint8_t getLength(); + PacketState getState(); + ResponseCode getResponseCode(); + + void unsupported(); + void overflow(); + + void sendReply(); + void sendPacket(); + void transmit(uint8_t d); + + void add_32(uint32_t d); + void add_16(uint16_t d); + void add_8(uint8_t d); + + uint8_t get_8(uint8_t idx); + uint16_t get_16(uint8_t idx); + uint32_t get_32(uint8_t idx); +}; + +#endif // _SIMPLE_PACKET_H_ diff --git a/tags/firmware/gen3/1.1/testers/imperial_stepper/imperial_stepper.pde b/tags/firmware/gen3/1.1/testers/imperial_stepper/imperial_stepper.pde new file mode 100644 index 00000000..857fdc2c --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/imperial_stepper/imperial_stepper.pde @@ -0,0 +1,206 @@ +// Yep, this is actually -*- c++ -*- + +//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 + +//pin for controlling the PSU. +#define PS_ON_PIN 14 + +void init_psu() +{ +#ifdef PS_ON_PIN + pinMode(PS_ON_PIN, OUTPUT); + turn_psu_on(); +#endif +} + +void turn_psu_on() +{ +#ifdef PS_ON_PIN + digitalWrite(PS_ON_PIN, LOW); + delay(2000); //wait for PSU to actually turn on. +#endif +} + +void turn_psu_off() +{ +#ifdef PS_ON_PIN + digitalWrite(PS_ON_PIN, HIGH); +#endif +} + +void setup() +{ + Serial.begin(38400); + Serial.println("You have failed me for the last time."); + + //initialize all our pins. + pinMode(X_STEP_PIN, OUTPUT); + pinMode(X_DIR_PIN, OUTPUT); + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(X_MIN_PIN, INPUT); + pinMode(X_MAX_PIN, INPUT); + + pinMode(Y_STEP_PIN, OUTPUT); + pinMode(Y_DIR_PIN, OUTPUT); + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Y_MIN_PIN, INPUT); + pinMode(Y_MAX_PIN, INPUT); + + pinMode(Z_STEP_PIN, OUTPUT); + pinMode(Z_DIR_PIN, OUTPUT); + pinMode(Z_ENABLE_PIN, OUTPUT); + pinMode(Z_MIN_PIN, INPUT); + pinMode(Z_MAX_PIN, INPUT); + + digitalWrite(X_MIN_PIN, HIGH); + digitalWrite(X_MAX_PIN, HIGH); + digitalWrite(Y_MIN_PIN, HIGH); + digitalWrite(Y_MAX_PIN, HIGH); + digitalWrite(Z_MIN_PIN, HIGH); + digitalWrite(Z_MAX_PIN, HIGH); + + init_psu(); + calculate_tones(); +} + +void loop() +{ + Serial.println("Forward!"); + digitalWrite(X_DIR_PIN, HIGH); + digitalWrite(Y_DIR_PIN, HIGH); + digitalWrite(Z_DIR_PIN, HIGH); + play_song(X_MAX_PIN, Y_MAX_PIN, Z_MAX_PIN); + + delay(500); + + Serial.println("Reverse!"); + digitalWrite(X_DIR_PIN, LOW); + digitalWrite(Y_DIR_PIN, LOW); + digitalWrite(Z_DIR_PIN, LOW); + play_song(X_MIN_PIN, Y_MIN_PIN, Z_MIN_PIN); + + delay(500); +} + +boolean at_switch(byte pin) +{ + return !digitalRead(pin); +} + +#define TONE_COUNT 27 + +float frequencies[TONE_COUNT] = { + 196.00, //G2 0 + 207.65, //G#2 1 + 220.00, //A2 2 + 233.08, //Bb2 3 + 246.94, //B2 4 + 261.63, //C3 5 + 277.18, //C#3 6 + 293.66, //D3 7 + 311.13, //D#3 8 + 329.63, //E3 9 + 349.23, //F3 10 + 369.99, //F#3 11 + 392.00, //G3 12 + 415.30, //G#3 13 + 440.00, //A3 14 + 466.16, //Bb3 15 + 493.88, //B3 16 + 523.25, //C4 17 + 554.37, //C#4 18 + 587.33, //D4 19 + 622.25, //D#4 20 + 659.26, //E4 21 + 698.46, //F4 22 + 739.99, //F#4 23 + 783.99, //G4 24 + 830.61, //G#4 25 + 880.00 //A4 26 +}; + + +int tones[TONE_COUNT]; + +#define NOTE_COUNT 66 +int notes[] = { + 12,12,12, 8,15,12, 8,15,12, // 9 + 19,19,19,20,15,12, 8,15,12, // 9 + 24,12,12,24,23,22,21,20,21, // 9 + 13,18,17,16,15,14,15, // 7 + 8,11, 8,11,15,12,15,19, // 8 + 24,12,12,24,23,22,21,20,21, // 9 + 13,18,17,16,15,14,15, // 7 + 8,11, 8,15,12, 8,15,12 // 8 +}; +int lengths[] = { + 4, 4, 4, 3, 1, 4, 3, 1, 8, + 4, 4, 4, 3, 1, 4, 3, 1, 8, + 4, 3, 1, 4, 3, 1, 1, 1, 4, + 2, 4, 3, 1, 1, 1, 4, + 2, 4, 3, 1, 4, 3, 1, 8, + 4, 3, 1, 4, 3, 1, 1, 1, 4, + 2, 4, 3, 1, 1, 1, 4, + 2, 4, 3, 1, 4, 3, 1, 8 +}; + +void calculate_tones() +{ + for (byte i=0; i<TONE_COUNT; i++) + tones[i] = (int)(1000000.0/ (2.0 * frequencies[i])); +} + +void play_song(byte xPin, byte yPin, byte zPin) +{ + digitalWrite(X_ENABLE_PIN, LOW); //enable + digitalWrite(Y_ENABLE_PIN, LOW); //enable + digitalWrite(Z_ENABLE_PIN, LOW); //enable + + for (byte i=0; i<NOTE_COUNT; i++) + { + if (!at_switch(xPin) && !at_switch(yPin) && !at_switch(zPin)) + { + play_note(tones[notes[i]], 80000*lengths[i]); + delay(10); + } + } + digitalWrite(X_ENABLE_PIN, HIGH); //disable + digitalWrite(Y_ENABLE_PIN, HIGH); //disable + digitalWrite(Z_ENABLE_PIN, HIGH); //disable +} + +void play_note(int note, long time) +{ + int count = round(time / note); + + for (int i=0; i<count; i++) + { + digitalWrite(X_STEP_PIN, HIGH); + digitalWrite(Y_STEP_PIN, HIGH); + digitalWrite(Z_STEP_PIN, HIGH); + delayMicroseconds(note); + digitalWrite(X_STEP_PIN, LOW); + digitalWrite(Y_STEP_PIN, LOW); + digitalWrite(Z_STEP_PIN, LOW); + delayMicroseconds(note); + } +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_master_pinger/rs485_master_pinger.pde b/tags/firmware/gen3/1.1/testers/rs485_master_pinger/rs485_master_pinger.pde new file mode 100644 index 00000000..74112226 --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_master_pinger/rs485_master_pinger.pde @@ -0,0 +1,78 @@ + +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 +#define DEBUG_PIN 0 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + //always enable RX. + digitalWrite(RX_ENABLE_PIN, LOW); + + Serial.begin(38400); + Serial1.begin(38400); + + transmit_mode(); +} + +void loop() +{ + for (byte i=32; i<128; i++) + { + digitalWrite(DEBUG_PIN, HIGH); + delay(100); + digitalWrite(DEBUG_PIN, LOW); + + Serial.print("OUT: "); + print_byte(i); + + rs485_tx(i); + + delay(900); + + //check for response + while (Serial1.available()) + { + byte b = Serial1.read(); + Serial.print("IN: "); + print_byte(i); + } + } +} + +void print_byte(byte b) +{ + Serial.print(b, BYTE); + Serial.print(" / "); + Serial.print(b, DEC); + Serial.print(" / "); + Serial.print(b, HEX); + Serial.print(" / "); + Serial.print(b, BIN); + Serial.println("."); +} + +void rs485_tx(byte b) +{ + transmit_mode(); + Serial1.print(b, BYTE); + + int tmp = Serial1.read(); + while (tmp != b) + tmp = Serial1.read(); + + receive_mode(); +} + +void transmit_mode() +{ + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx +} + +void receive_mode() +{ + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_master_receiver/rs485_master_receiver.pde b/tags/firmware/gen3/1.1/testers/rs485_master_receiver/rs485_master_receiver.pde new file mode 100644 index 00000000..51754b0a --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_master_receiver/rs485_master_receiver.pde @@ -0,0 +1,36 @@ + +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 +#define DEBUG_PIN 0 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + Serial.begin(38400); + Serial.println("Started"); + Serial1.begin(38400); + + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + digitalWrite(RX_ENABLE_PIN, LOW); //enable rx +} + +void loop() +{ + if (Serial1.available() > 0) + { + int b = Serial1.read(); + + Serial.print("IN: "); + Serial.print(b, BYTE); + Serial.print(" / "); + Serial.print(b, DEC); + Serial.print(" / "); + Serial.print(b, HEX); + Serial.print(" / "); + Serial.print(b, BIN); + Serial.println("."); + } +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_master_sender/rs485_master_sender.pde b/tags/firmware/gen3/1.1/testers/rs485_master_sender/rs485_master_sender.pde new file mode 100644 index 00000000..13baab37 --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_master_sender/rs485_master_sender.pde @@ -0,0 +1,57 @@ + +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 +#define DEBUG_PIN 0 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + Serial.begin(38400); + Serial1.begin(38400); + + transmit_mode(); +} + +void loop() +{ + for (byte i=32; i<128; i++) + { + digitalWrite(DEBUG_PIN, HIGH); + delay(100); + digitalWrite(DEBUG_PIN, LOW); + + Serial.print("OUT: "); + print_byte(i); + + Serial1.print(i, BYTE); + + delay(900); + } +} + +void print_byte(byte b) +{ + Serial.print(b, BYTE); + Serial.print(" / "); + Serial.print(b, DEC); + Serial.print(" / "); + Serial.print(b, HEX); + Serial.print(" / "); + Serial.print(b, BIN); + Serial.println("."); +} + +void transmit_mode() +{ + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + digitalWrite(RX_ENABLE_PIN, HIGH); //disable rx +} + +void receive_mode() +{ + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + digitalWrite(RX_ENABLE_PIN, LOW); //enable rx +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_slave_pingee/rs485_slave_pingee.pde b/tags/firmware/gen3/1.1/testers/rs485_slave_pingee/rs485_slave_pingee.pde new file mode 100644 index 00000000..6f0b3c9b --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_slave_pingee/rs485_slave_pingee.pde @@ -0,0 +1,53 @@ +#define RX_ENABLE_PIN 4 +#define TX_ENABLE_PIN 16 +#define DEBUG_PIN 13 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + digitalWrite(DEBUG_PIN, LOW); //disable led + + digitalWrite(RX_ENABLE_PIN, LOW); //always listen. + + Serial.begin(38400); + +} + +void loop() +{ + if (Serial.available()) + { + byte i = Serial.read(); + + rs485_tx(i); + + digitalWrite(DEBUG_PIN, HIGH); + delayMicroseconds(100); + digitalWrite(DEBUG_PIN, LOW); + } +} + +void rs485_tx(byte b) +{ + transmit_mode(); + Serial.print(b, BYTE); + + int tmp = Serial.read(); + while (tmp != b) + tmp = Serial.read(); + + receive_mode(); +} + +void transmit_mode() +{ + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx +} + +void receive_mode() +{ + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_slave_receiver/rs485_slave_receiver.pde b/tags/firmware/gen3/1.1/testers/rs485_slave_receiver/rs485_slave_receiver.pde new file mode 100644 index 00000000..482afcb6 --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_slave_receiver/rs485_slave_receiver.pde @@ -0,0 +1,39 @@ +#define RX_ENABLE_PIN 4 +#define TX_ENABLE_PIN 16 +#define DEBUG_PIN 13 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + digitalWrite(DEBUG_PIN, LOW); //disable led + + Serial.begin(38400); + receive_mode(); +} + +void loop() +{ + if (Serial.available()) + { + byte i = Serial.read(); + + digitalWrite(DEBUG_PIN, HIGH); + delayMicroseconds(100); + digitalWrite(DEBUG_PIN, LOW); + } +} + +void transmit_mode() +{ + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + digitalWrite(RX_ENABLE_PIN, HIGH); //disable rx +} + +void receive_mode() +{ + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + digitalWrite(RX_ENABLE_PIN, LOW); //enable rx +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_slave_sender/rs485_slave_sender.pde b/tags/firmware/gen3/1.1/testers/rs485_slave_sender/rs485_slave_sender.pde new file mode 100644 index 00000000..9798c2ab --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_slave_sender/rs485_slave_sender.pde @@ -0,0 +1,32 @@ +#define RX_ENABLE_PIN 4 +#define TX_ENABLE_PIN 16 +#define DEBUG_PIN 13 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + digitalWrite(RX_ENABLE_PIN, HIGH); //disable rx + digitalWrite(DEBUG_PIN, LOW); //disable led + + Serial.begin(38400); +} + +void loop() +{ + for (int i=32; i<127; i++) + { + Serial.print(i, BYTE); + Serial.print(i, BYTE); + Serial.print(i, BYTE); + Serial.print(i, BYTE); + + digitalWrite(DEBUG_PIN, HIGH); + delay(100); + digitalWrite(DEBUG_PIN, LOW); + delay(900); + } +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_tester_master/rs485_tester_master.pde b/tags/firmware/gen3/1.1/testers/rs485_tester_master/rs485_tester_master.pde new file mode 100644 index 00000000..597c3a0e --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_tester_master/rs485_tester_master.pde @@ -0,0 +1,36 @@ + +#define RX_ENABLE_PIN 13 +#define TX_ENABLE_PIN 12 +#define DEBUG_PIN 0 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + Serial.begin(38400); + Serial1.begin(38400); +} + +void loop() +{ + digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx + digitalWrite(RX_ENABLE_PIN, HIGH); //disable rx + digitalWrite(DEBUG_PIN, LOW); //disable led + + Serial.println("started"); + Serial1.println("hello?"); + + //re-enable rx, disable tx + digitalWrite(RX_ENABLE_PIN, LOW); + digitalWrite(TX_ENABLE_PIN, LOW); + + if (Serial1.available() > 0) + { + digitalWrite(DEBUG_PIN, HIGH); + Serial.println("RS485 active."); + } + + delay(2000); +} diff --git a/tags/firmware/gen3/1.1/testers/rs485_tester_slave/rs485_tester_slave.pde b/tags/firmware/gen3/1.1/testers/rs485_tester_slave/rs485_tester_slave.pde new file mode 100644 index 00000000..4b42ee35 --- /dev/null +++ b/tags/firmware/gen3/1.1/testers/rs485_tester_slave/rs485_tester_slave.pde @@ -0,0 +1,25 @@ +#define RX_ENABLE_PIN 4 +#define TX_ENABLE_PIN 16 +#define DEBUG_PIN 13 + +void setup() +{ + pinMode(RX_ENABLE_PIN, OUTPUT); + pinMode(TX_ENABLE_PIN, OUTPUT); + pinMode(DEBUG_PIN, OUTPUT); + + digitalWrite(TX_ENABLE_PIN, LOW); //disable tx + digitalWrite(RX_ENABLE_PIN, LOW); //enable rx + digitalWrite(DEBUG_PIN, LOW); //disable led + + Serial.begin(38400); +} + +void loop() +{ + if (Serial.available() > 0) + { + digitalWrite(DEBUG_PIN, HIGH); + Serial.println("RS485 active."); + } +} |