gamma_min 0 #
gamma_max 200 #
+# optional enable limit switches, actions will stop if any enabled limit switch is triggered
+#alpha_limit_enable false # set to true to enable X min and max limit switches
+#beta_limit_enable false # set to true to enable Y min and max limit switches
+#gamma_limit_enable false # set to true to enable Z min and max limit switches
+
#probe endstop
#probe_pin 1.29 # optional pin for probe
mm_per_arc_segment 0.5 # Arcs are cut into segments ( lines ), this is the length for
# these segments. Smaller values mean more resolution,
# higher values mean faster computation
-#mm_per_line_segment 0.5 # Lines can be cut into segments ( not usefull with cartesian
+#mm_per_line_segment 0.5 # Lines can be cut into segments ( not useful with cartesian
# coordinates robots ).
delta_segments_per_second 100 # for deltas only same as in Marlin/Delta, set to 0 to disable
# and use mm_per_line_segment
gamma_steps_per_mm 100 # Steps per mm for gamma stepper
arm_length 250.0 # this is the length of an arm from hinge to hinge
-arm_radius 124.0 # this is the horiontal distance from hinge to hinge
+arm_radius 124.0 # this is the horizontal distance from hinge to hinge
# when the effector is centered
# Planner module configuration : Look-ahead and acceleration configuration
extruder.hotend.dir_pin 0.22 # Pin for extruder dir signal
extruder.hotend.en_pin 0.21 # Pin for extruder enable signal
-# for now the first extruder's offsets should be left at 0 or homing will get confused
+# extruder offset
#extruder.hotend.x_offset 0 # x offset from origin in mm
#extruder.hotend.y_offset 0 # y offset from origin in mm
#extruder.hotend.z_offset 0 # z offset from origin in mm
+# firmware retract settings when using G10/G11, these are the defaults if not defined, must be defined for each extruder if not using the defaults
+#extruder.hotend.retract_length 3 # retract length in mm
+#extruder.hotend.retract_feedrate 45 # retract feedrate in mm/sec
+#extruder.hotend.retract_recover_length 0 # additional length for recover
+#extruder.hotend.retract_recover_feedrate 8 # recover feedrate in mm/sec (should be less than retract feedrate)
+#extruder.hotend.retract_zlift_length 0 # zlift on retract in mm, 0 disables
+#extruder.hotend.retract_zlift_feedrate 6000 # zlift feedrate in mm/min (Note mm/min NOT mm/sec)
+
delta_current 1.5 # First extruder stepper motor current
# Second extruder module configuration example
extruder.hotend.dir_pin 0.22 # Pin for extruder dir signal
extruder.hotend.en_pin 0.21 # Pin for extruder enable signal
-# for now the first extruder's offsets should be left at 0 or homing will get confused
+# extruder offset
#extruder.hotend.x_offset 0 # x offset from origin in mm
#extruder.hotend.y_offset 0 # y offset from origin in mm
#extruder.hotend.z_offset 0 # z offset from origin in mm
+# firmware retract settings when using G10/G11, these are the defaults if not defined, must be defined for each extruder if not using the defaults
+#extruder.hotend.retract_length 3 # retract length in mm
+#extruder.hotend.retract_feedrate 45 # retract feedrate in mm/sec
+#extruder.hotend.retract_recover_length 0 # additional length for recover
+#extruder.hotend.retract_recover_feedrate 8 # recover feedrate in mm/sec (should be less than retract feedrate)
+#extruder.hotend.retract_zlift_length 0 # zlift on retract in mm, 0 disables
+#extruder.hotend.retract_zlift_feedrate 6000 # zlift feedrate in mm/min (Note mm/min NOT mm/sec)
+
delta_current 1.5 # First extruder stepper motor current
# Second extruder module configuration
#extruder.hotend2.z_offset 0 # z offset from origin in mm
#epsilon_current 1.5 # Second extruder stepper motor current
+
# Laser module configuration
laser_module_enable false # Whether to activate the laser module at all. All configuration is
# ignored if false.
endstops_enable true # the endstop module is enabled by default and can be disabled here
#corexy_homing false # set to true if homing on a hbit or corexy
alpha_min_endstop 1.24^ # add a ! to invert if endstop is NO connected to ground
-alpha_max_endstop 1.25^ #
+alpha_max_endstop 1.25^ # NOTE set to nc if this is not installed
alpha_homing_direction home_to_min # or set to home_to_max and set alpha_max
alpha_min 0 # this gets loaded after homing when home_to_min is set
alpha_max 200 # this gets loaded after homing when home_to_max is set
gamma_min 0 #
gamma_max 200 #
+# optional enable limit switches, actions will stop if any enabled limit switch is triggered
+#alpha_limit_enable false # set to true to enable X min and max limit switches
+#beta_limit_enable false # set to true to enable Y min and max limit switches
+#gamma_limit_enable false # set to true to enable Z min and max limit switches
+
alpha_fast_homing_rate_mm_s 50 # feedrates in mm/second
beta_fast_homing_rate_mm_s 50 # "
gamma_fast_homing_rate_mm_s 4 # "
--- /dev/null
+223b19e77f621f579e9663f8761bafd1 FirmwareBin/firmware.bin
#define network_telnet_checksum CHECKSUM("telnet")
#define network_mac_override_checksum CHECKSUM("mac_override")
#define network_ip_address_checksum CHECKSUM("ip_address")
+#define network_hostname_checksum CHECKSUM("hostname")
#define network_ip_gateway_checksum CHECKSUM("ip_gateway")
#define network_ip_mask_checksum CHECKSUM("ip_mask")
theNetwork= this;
sftpd= NULL;
instance= this;
+ hostname = NULL;
}
Network::~Network()
{
delete ethernet;
+ if (hostname != NULL) {
+ delete hostname;
+ }
}
static uint32_t getSerialNumberHash()
return true;
}
+static bool parse_hostname(const string &s)
+{
+ const std::string::size_type str_len = s.size();
+ if(str_len > 63){
+ return false;
+ }
+ for (unsigned int i = 0; i < str_len; i++) {
+ const char c = s.at(i);
+ if(!(c >= 'a' && c <= 'z')
+ && !(c >= 'A' && c <= 'Z')
+ && !(i != 0 && c >= '0' && c <= '9')
+ && !(i != 0 && i != str_len - 1 && c == '-')){
+ return false;
+ }
+ }
+ return true;
+}
+
void Network::on_module_loaded()
{
if ( !THEKERNEL->config->value( network_checksum, network_enable_checksum )->by_default(false)->as_bool() ) {
ethernet->set_mac(mac_address);
// get IP address, mask and gateway address here....
- bool bad = false;
string s = THEKERNEL->config->value( network_checksum, network_ip_address_checksum )->by_default("auto")->as_string();
if (s == "auto") {
use_dhcp = true;
-
+ s = THEKERNEL->config->value( network_checksum, network_hostname_checksum )->as_string();
+ if (!s.empty()) {
+ if(parse_hostname(s)){
+ hostname = new char [s.length() + 1];
+ strcpy(hostname, s.c_str());
+ }else{
+ printf("Invalid hostname: %s\n", s.c_str());
+ }
+ }
} else {
+ bool bad = false;
use_dhcp = false;
if (!parse_ip_str(s, ipaddr, 4)) {
printf("Invalid IP address: %s\n", s.c_str());
printf("Invalid IP gateway: %s\n", s.c_str());
bad = true;
}
-
if (bad) {
printf("Network not started due to errors in config");
return;
}else{
#if UIP_CONF_UDP
- dhcpc_init(mac_address, sizeof(mac_address));
+ dhcpc_init(mac_address, sizeof(mac_address), hostname);
dhcpc_request();
printf("Getting IP address....\n");
#endif
uint8_t ipaddr[4];
uint8_t ipmask[4];
uint8_t ipgw[4];
+ char *hostname;
volatile uint32_t tickcnt;
};
#define DHCP_OPTION_SUBNET_MASK 1
#define DHCP_OPTION_ROUTER 3
#define DHCP_OPTION_DNS_SERVER 6
+#define DHCP_OPTION_HOSTNAME 12
#define DHCP_OPTION_REQ_IPADDR 50
#define DHCP_OPTION_LEASE_TIME 51
#define DHCP_OPTION_MSG_TYPE 53
}
/*---------------------------------------------------------------------------*/
static u8_t *
+add_hostname(u8_t *optptr)
+{
+ if (s.hostname == NULL) {
+ return optptr;
+ }
+ const u8_t l = strlen(s.hostname);
+ *optptr++ = DHCP_OPTION_HOSTNAME;
+ *optptr++ = l;
+ memcpy(optptr, s.hostname, l);
+ return optptr + l;
+}
+/*---------------------------------------------------------------------------*/
+static u8_t *
add_req_options(u8_t *optptr)
{
*optptr++ = DHCP_OPTION_REQ_LIST;
- *optptr++ = 3;
+ *optptr++ = s.hostname == NULL ? 3 : 4;
*optptr++ = DHCP_OPTION_SUBNET_MASK;
*optptr++ = DHCP_OPTION_ROUTER;
*optptr++ = DHCP_OPTION_DNS_SERVER;
+ if (s.hostname != NULL) {
+ *optptr++ = DHCP_OPTION_HOSTNAME;
+ }
return optptr;
}
/*---------------------------------------------------------------------------*/
end = add_msg_type(&m->options[4], DHCPREQUEST);
end = add_server_id(end);
end = add_req_ipaddr(end);
+ end = add_hostname(end);
end = add_end(end);
uip_send(uip_appdata, end - (u8_t *)uip_appdata);
}
/*---------------------------------------------------------------------------*/
void
-dhcpc_init(const void *mac_addr, int mac_len)
+dhcpc_init(const void *mac_addr, int mac_len, char *hostname)
{
uip_ipaddr_t addr;
s.mac_addr = mac_addr;
s.mac_len = mac_len;
+ s.hostname = hostname;
s.state = STATE_INITIAL;
uip_ipaddr(addr, 255, 255, 255, 255);
u16_t ticks;
const void *mac_addr;
int mac_len;
-
+ char *hostname;
u8_t serverid[4];
uint32_t lease_time;
extern "C" {
#endif
-void dhcpc_init(const void *mac_addr, int mac_len);
+void dhcpc_init(const void *mac_addr, int mac_len, char *hostname);
void dhcpc_request(void);
void dhcpc_appcall(void);
#include "modules/tools/endstops/Endstops.h"
#include "modules/tools/touchprobe/Touchprobe.h"
#include "modules/tools/zprobe/ZProbe.h"
+#include "modules/tools/scaracal/SCARAcal.h"
#include "modules/tools/switch/SwitchPool.h"
#include "modules/tools/temperatureswitch/TemperatureSwitch.h"
delete sp;
#endif
#ifndef NO_TOOLS_EXTRUDER
+ // NOTE this must be done first before Temperature control so ToolManager can handle Tn before temperaturecontrol module does
ExtruderMaker *em= new ExtruderMaker();
em->load_tools();
delete em;
#endif
#ifndef NO_TOOLS_TEMPERATURECONTROL
- // Note order is important here must be after extruder
+ // Note order is important here must be after extruder so Tn as a parameter will get executed first
TemperatureControlPool *tp= new TemperatureControlPool();
tp->load_tools();
delete tp;
#ifndef NO_TOOLS_ZPROBE
kernel->add_module( new ZProbe() );
#endif
+ #ifndef NO_TOOLS_SCARACAL
+ kernel->add_module( new SCARAcal() );
+ #endif
#ifndef NONETWORK
kernel->add_module( new Network() );
#endif
#include <string>
using namespace std;
-// The Pauser module is the core of the pausing subsystem in smoothie. Basically we want several modules to be able to pause smoothie at the same time
-// ( think both the user with a button, and the temperature control because a temperature is not reached ). To do that, modules call the take() methode,
-// a pause event is called, and the pause does not end before all modules have called the release() method.
-// Please note : Modules should keep track of their pause status themselves
+// The Pauser module is the core of the pausing subsystem in smoothie.
+// Basically we want several modules to be able to pause smoothie at the same
+// time ( think both the user with a button, and the temperature control
+// because a temperature is not reached ). To do that, modules call the take()
+// methode, a pause event is called, and the pause does not end before all
+// modules have called the release() method.
+// Please note : Modules should keep track of their pause status themselves...
+// or Not for instance it may be better for the play/pause button to be able
+// to unpause when something else causes a pause
Pauser::Pauser(){
paused_block = NULL;
}
// Unpause smoothie unless something else is pausing it too
void Pauser::release(){
- this->counter--;
- //THEKERNEL->streams->printf("release: %u \r\n", this->counter );
- if( this->counter == 0 ){
+ if( --this->counter <= 0 ){
+ this->counter= 0;
THEKERNEL->call_event(ON_PLAY, &this->counter);
if (paused_block)
{
bool paused();
+ private:
Block* paused_block;
unsigned short counter;
};
#include "arm_solutions/RotatableCartesianSolution.h"
#include "arm_solutions/LinearDeltaSolution.h"
#include "arm_solutions/HBotSolution.h"
+#include "arm_solutions/MorganSCARASolution.h"
#include "StepTicker.h"
#include "checksumm.h"
#include "utils.h"
#define hbot_checksum CHECKSUM("hbot")
#define corexy_checksum CHECKSUM("corexy")
#define kossel_checksum CHECKSUM("kossel")
+#define morgan_checksum CHECKSUM("morgan")
// stepper motor stuff
#define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
} else if(solution_checksum == rotatable_cartesian_checksum) {
this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
+ } else if(solution_checksum == morgan_checksum) {
+ this->arm_solution = new MorganSCARASolution(THEKERNEL->config);
+
} else if(solution_checksum == cartesian_checksum) {
this->arm_solution = new CartesianSolution(THEKERNEL->config);
float to_millimeters(float value);
float from_millimeters(float value);
float get_seconds_per_minute() const { return seconds_per_minute; }
+ float get_z_maxfeedrate() const { return this->max_speeds[2]; }
BaseSolution* arm_solution; // Selected Arm solution ( millimeters to step calculation )
bool absolute_mode; // true for absolute mode ( default ), false for relative mode
--- /dev/null
+#include "MorganSCARASolution.h"
+#include <fastmath.h>
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "libs/Kernel.h"
+//#include "StreamOutputPool.h"
+//#include "Gcode.h"
+//#include "SerialMessage.h"
+//#include "Conveyor.h"
+//#include "Robot.h"
+//#include "StepperMotor.h"
+
+#include "libs/nuts_bolts.h"
+
+#include "libs/Config.h"
+
+#define arm1_length_checksum CHECKSUM("arm1_length")
+#define arm2_length_checksum CHECKSUM("arm2_length")
+#define morgan_offset_x_checksum CHECKSUM("morgan_offset_x")
+#define morgan_offset_y_checksum CHECKSUM("morgan_offset_y")
+#define axis_scaling_x_checksum CHECKSUM("axis_scaling_x")
+#define axis_scaling_y_checksum CHECKSUM("axis_scaling_y")
+
+#define SQ(x) powf(x, 2)
+#define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
+
+MorganSCARASolution::MorganSCARASolution(Config* config)
+{
+ // arm1_length is the length of the inner main arm from hinge to hinge
+ arm1_length = config->value(arm1_length_checksum)->by_default(150.0f)->as_number();
+ // arm2_length is the length of the inner main arm from hinge to hinge
+ arm2_length = config->value(arm2_length_checksum)->by_default(150.0f)->as_number();
+ // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
+ morgan_offset_x = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
+ // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
+ morgan_offset_y = config->value(morgan_offset_y_checksum)->by_default(-65.0f)->as_number();
+
+ init();
+}
+
+void MorganSCARASolution::init() {
+
+}
+
+float MorganSCARASolution::to_degrees(float radians) {
+ return radians*(180.0F/3.14159265359f);
+}
+
+void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
+{
+
+ float SCARA_pos[2],
+ SCARA_C2,
+ SCARA_S2,
+ SCARA_K1,
+ SCARA_K2,
+ SCARA_theta,
+ SCARA_psi;
+
+ SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x; //Translate cartesian to tower centric SCARA X Y
+ SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y; // morgan_offset not to be confused with home offset. Makes the SCARA math work.
+
+ if (this->arm1_length == this->arm2_length)
+ SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
+ else
+ SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));
+
+ SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
+
+ SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
+ SCARA_K2 = this->arm2_length*SCARA_S2;
+
+ SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction
+ SCARA_psi = atan2f(SCARA_S2,SCARA_C2);
+
+
+ actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle
+ actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
+ //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara
+ actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; // No inverse kinematics on Z - Position to add bed offset?
+
+}
+
+void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
+ // Perform forward kinematics, and place results in cartesian_mm[]
+
+ float y1, y2,
+ actuator_rad[2];
+
+ actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
+ actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);
+
+ y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
+ y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
+
+ cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x;
+ cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
+ cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
+
+ cartesian_mm[0] = ROUND(cartesian_mm[0], 4);
+ cartesian_mm[1] = ROUND(cartesian_mm[1], 4);
+ cartesian_mm[2] = ROUND(cartesian_mm[2], 4);
+}
+
+bool MorganSCARASolution::set_optional(const arm_options_t& options) {
+
+ arm_options_t::const_iterator i;
+
+ i= options.find('T'); // Theta arm1 length
+ if(i != options.end()) {
+ arm1_length= i->second;
+
+ }
+ i= options.find('P'); // Psi arm2 length
+ if(i != options.end()) {
+ arm2_length= i->second;
+ }
+ i= options.find('X'); // Home initial position X
+ if(i != options.end()) {
+ morgan_offset_x= i->second;
+ }
+ i= options.find('Y'); // Home initial position Y
+ if(i != options.end()) {
+ morgan_offset_y= i->second;
+ }
+
+ init();
+ return true;
+}
+
+bool MorganSCARASolution::get_optional(arm_options_t& options) {
+ options['T']= this->arm1_length;
+ options['P']= this->arm2_length;
+ options['X']= this->morgan_offset_x;
+ options['Y']= this->morgan_offset_y;
+ return true;
+};
--- /dev/null
+#ifndef MORGANSCARASOLUTION_H
+#define MORGANSCARASOLUTION_H
+//#include "libs/Module.h"
+#include "BaseSolution.h"
+
+class Config;
+
+class MorganSCARASolution : public BaseSolution {
+ public:
+ MorganSCARASolution(Config*);
+ void cartesian_to_actuator( float[], float[] );
+ void actuator_to_cartesian( float[], float[] );
+
+ bool set_optional(const arm_options_t& options);
+ bool get_optional(arm_options_t& options);
+
+ private:
+ void init();
+ float to_degrees(float radians);
+
+ float arm1_length;
+ float arm2_length;
+ float morgan_offset_x;
+ float morgan_offset_y;
+ float slow_rate;
+};
+
+#endif // MORGANSCARASOLUTION_H
#include "libs/StreamOutput.h"
#include "PublicDataRequest.h"
#include "EndstopsPublicAccess.h"
+#include "StreamOutputPool.h"
+#include "Pauser.h"
#define ALPHA_AXIS 0
#define BETA_AXIS 1
#define Y_AXIS 1
#define Z_AXIS 2
-#define NOT_HOMING 0
-#define MOVING_TO_ORIGIN_FAST 1
-#define MOVING_BACK 2
-#define MOVING_TO_ORIGIN_SLOW 3
-
#define endstops_module_enable_checksum CHECKSUM("endstops_enable")
#define corexy_homing_checksum CHECKSUM("corexy_homing")
#define delta_homing_checksum CHECKSUM("delta_homing")
#define beta_max_checksum CHECKSUM("beta_max")
#define gamma_max_checksum CHECKSUM("gamma_max")
+#define alpha_limit_enable_checksum CHECKSUM("alpha_limit_enable")
+#define beta_limit_enable_checksum CHECKSUM("beta_limit_enable")
+#define gamma_limit_enable_checksum CHECKSUM("gamma_limit_enable")
+
#define STEPPER THEKERNEL->robot->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
+// Homing States
+enum{
+ NOT_HOMING,
+ MOVING_TO_ORIGIN_FAST,
+ MOVING_BACK,
+ MOVING_TO_ORIGIN_SLOW,
+ BACK_OFF_HOME,
+ LIMIT_TRIGGERED
+};
+
Endstops::Endstops()
{
this->status = NOT_HOMING;
this->trim_mm[0] = THEKERNEL->config->value(alpha_trim_checksum )->by_default(0 )->as_number();
this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum )->by_default(0 )->as_number();
this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum )->by_default(0 )->as_number();
+
+ // limits enabled
+ this->limit_enable[X_AXIS]= THEKERNEL->config->value(alpha_limit_enable_checksum)->by_default(false)->as_bool();
+ this->limit_enable[Y_AXIS]= THEKERNEL->config->value(beta_limit_enable_checksum)->by_default(false)->as_bool();
+ this->limit_enable[Z_AXIS]= THEKERNEL->config->value(gamma_limit_enable_checksum)->by_default(false)->as_bool();
+
+ if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]){
+ register_for_event(ON_IDLE);
+ }
+}
+
+static const char *endstop_names[]= {"MIN_X", "MIN_Y", "MIN_Z", "MAX_X", "MAX_Y", "MAX_Z"};
+
+void Endstops::on_idle(void *argument)
+{
+ if(this->status != NOT_HOMING) return; // don't check while homing or if a LIMIT was triggered
+
+ for( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+ if(this->limit_enable[c] && STEPPER[c]->is_moving()) {
+ std::array<int, 2> minmax{{0, 3}};
+ // check min and max endstops
+ for (int i : minmax) {
+ int n= c+i;
+ uint8_t debounce= 0;
+ while(this->pins[n].get()) {
+ if ( ++debounce >= debounce_count ) {
+ // endstop triggered
+ THEKERNEL->pauser->take();
+ THEKERNEL->streams->printf("Limit switch %s was hit - reset or press play button\n", endstop_names[n]);
+ this->status= LIMIT_TRIGGERED;
+ return;
+ }
+ }
+ }
+ }
+ }
+}
+
+// if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move
+// TODO should check if triggered and only back off if triggered
+void Endstops::back_off_home()
+{
+ this->status = BACK_OFF_HOME;
+ for( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+ if(this->limit_enable[c]) {
+ // Move off of the endstop using a regular relative move
+ char buf[32];
+ snprintf(buf, sizeof(buf), "G0 %c%1.4f F%1.4f", c+'X', this->retract_mm[c]*(this->home_direction[c]?1:-1), this->slow_rates[c]*60.0F);
+ Gcode gc(buf, &(StreamOutput::NullStream));
+ bool oldmode= THEKERNEL->robot->absolute_mode;
+ THEKERNEL->robot->absolute_mode= false; // needs to be relative mode
+ THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+ THEKERNEL->robot->absolute_mode= oldmode; // restore mode
+ }
+ }
+ // Wait for above to finish
+ THEKERNEL->conveyor->wait_for_empty_queue();
+ this->status = NOT_HOMING;
}
void Endstops::wait_for_homed(char axes_to_move)
THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
}
}
+
+ // if limit switches are enabled we must back off endstop after setting home
+ // TODO should maybe be done before setting home so X0 does not retrigger?
+ back_off_home();
}
} else if (gcode->has_m) {
switch (gcode->m) {
case 119: {
-
- int px = this->home_direction[0] ? 0 : 3;
- int py = this->home_direction[1] ? 1 : 4;
- int pz = this->home_direction[2] ? 2 : 5;
- const char *mx = this->home_direction[0] ? "min" : "max";
- const char *my = this->home_direction[1] ? "min" : "max";
- const char *mz = this->home_direction[2] ? "min" : "max";
-
- gcode->stream->printf("X %s:%d Y %s:%d Z %s:%d", mx, this->pins[px].get(), my, this->pins[py].get(), mz, this->pins[pz].get());
+ for (int i = 0; i < 6; ++i) {
+ if(this->pins[i].connected())
+ gcode->stream->printf("%s:%d ", endstop_names[i], this->pins[i].get());
+ }
gcode->add_nl= true;
gcode->mark_as_taken();
}
void wait_for_homed(char axes_to_move);
void wait_for_homed_corexy(int axis);
void corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps);
+ void back_off_home();
void on_get_public_data(void* argument);
void on_set_public_data(void* argument);
+ void on_idle(void *argument);
float homing_position[3];
float home_offset[3];
std::bitset<3> home_direction;
+ std::bitset<3> limit_enable;
+
unsigned int debounce_count;
float retract_mm[3];
float trim_mm[3];
#include <mri.h>
+// OLD config names for backwards compatibility, NOTE new configs will not be added here
#define extruder_module_enable_checksum CHECKSUM("extruder_module_enable")
#define extruder_steps_per_mm_checksum CHECKSUM("extruder_steps_per_mm")
#define extruder_filament_diameter_checksum CHECKSUM("extruder_filament_diameter")
#define extruder_en_pin_checksum CHECKSUM("extruder_en_pin")
#define extruder_max_speed_checksum CHECKSUM("extruder_max_speed")
+// NEW config names
#define extruder_checksum CHECKSUM("extruder")
#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
#define y_offset_checksum CHECKSUM("y_offset")
#define z_offset_checksum CHECKSUM("z_offset")
+#define retract_length_checksum CHECKSUM("retract_length")
+#define retract_feedrate_checksum CHECKSUM("retract_feedrate")
+#define retract_recover_length_checksum CHECKSUM("retract_recover_length")
+#define retract_recover_feedrate_checksum CHECKSUM("retract_recover_feedrate")
+#define retract_zlift_length_checksum CHECKSUM("retract_zlift_length")
+#define retract_zlift_feedrate_checksum CHECKSUM("retract_zlift_feedrate")
+
#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2
this->paused = false;
this->single_config = single;
this->identifier = config_identifier;
+ this->retracted = false;
+ this->volumetric_multiplier= 1.0F;
memset(this->offset, 0, sizeof(this->offset));
}
// Stepper motor object for the extruder
this->stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(step_pin, dir_pin, en_pin) );
this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move );
-
}
// Get config
if( this->single_config ) {
// If this module uses the old "single extruder" configuration style
- this->steps_per_millimeter_setting = THEKERNEL->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number();
+ this->steps_per_millimeter = THEKERNEL->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number();
this->filament_diameter = THEKERNEL->config->value(extruder_filament_diameter_checksum )->by_default(0)->as_number();
this->acceleration = THEKERNEL->config->value(extruder_acceleration_checksum )->by_default(1000)->as_number();
this->max_speed = THEKERNEL->config->value(extruder_max_speed_checksum )->by_default(1000)->as_number();
} else {
// If this module was created with the new multi extruder configuration style
- this->steps_per_millimeter_setting = THEKERNEL->config->value(extruder_checksum, this->identifier, steps_per_mm_checksum )->by_default(1)->as_number();
+ this->steps_per_millimeter = THEKERNEL->config->value(extruder_checksum, this->identifier, steps_per_mm_checksum )->by_default(1)->as_number();
this->filament_diameter = THEKERNEL->config->value(extruder_checksum, this->identifier, filament_diameter_checksum )->by_default(0)->as_number();
this->acceleration = THEKERNEL->config->value(extruder_checksum, this->identifier, acceleration_checksum )->by_default(1000)->as_number();
this->max_speed = THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum )->by_default(1000)->as_number();
this->offset[X_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, x_offset_checksum )->by_default(0)->as_number();
this->offset[Y_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, y_offset_checksum )->by_default(0)->as_number();
this->offset[Z_AXIS] = THEKERNEL->config->value(extruder_checksum, this->identifier, z_offset_checksum )->by_default(0)->as_number();
+
}
- this->update_steps_per_millimeter();
+ // these are only supported in the new syntax, no need to be backward compatible as they did not exist before the change
+ this->retract_length = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_length_checksum)->by_default(3)->as_number();
+ this->retract_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_feedrate_checksum)->by_default(45)->as_number();
+ this->retract_recover_length = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_length_checksum)->by_default(0)->as_number();
+ this->retract_recover_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_feedrate_checksum)->by_default(8)->as_number();
+ this->retract_zlift_length = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_length_checksum)->by_default(0)->as_number();
+ this->retract_zlift_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_feedrate_checksum)->by_default(100*60)->as_number(); // mm/min
+
+ if(filament_diameter > 0.01) {
+ this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
+ }
}
void Extruder::on_get_public_data(void* argument){
if(this->enabled) {
// Note this is allowing both step/mm and filament diameter to be exposed via public data
- pdr->set_data_ptr(&this->steps_per_millimeter_setting);
+ pdr->set_data_ptr(&this->steps_per_millimeter);
pdr->set_taken();
}
}
{
Gcode *gcode = static_cast<Gcode *>(argument);
- // Gcodes to execute immediately
+ // M codes most execute immediately, most only execute if enabled
if (gcode->has_m) {
if (gcode->m == 114 && this->enabled) {
char buf[16];
float spm = this->steps_per_millimeter;
if (gcode->has_letter('E')) {
spm = gcode->get_value('E');
- this->steps_per_millimeter_setting = spm;
- this->update_steps_per_millimeter();
+ this->steps_per_millimeter = spm;
}
gcode->stream->printf("E:%g ", spm);
} else if (gcode->m == 200 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
if (gcode->has_letter('D')) {
+ THEKERNEL->conveyor->wait_for_empty_queue(); // only apply after the queue has emptied
this->filament_diameter = gcode->get_value('D');
- this->update_steps_per_millimeter();
+ if(filament_diameter > 0.01) {
+ this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI);
+ }else{
+ this->volumetric_multiplier = 1.0F;
+ }
}
gcode->mark_as_taken();
+ } else if (gcode->m == 207 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
+ // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] Q[zlift feedrate mm/min]
+ if(gcode->has_letter('S')) retract_length = gcode->get_value('S');
+ if(gcode->has_letter('F')) retract_feedrate = gcode->get_value('F')/60.0F; // specified in mm/min converted to mm/sec
+ if(gcode->has_letter('Z')) retract_zlift_length = gcode->get_value('Z');
+ if(gcode->has_letter('Q')) retract_zlift_feedrate = gcode->get_value('Q');
+ gcode->mark_as_taken();
+
+ } else if (gcode->m == 208 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) {
+ // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
+ if(gcode->has_letter('S')) retract_recover_length = gcode->get_value('S');
+ if(gcode->has_letter('F')) retract_recover_feedrate = gcode->get_value('F')/60.0F; // specified in mm/min converted to mm/sec
+ gcode->mark_as_taken();
+
} else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
if( this->single_config ) {
- gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter_setting);
+ gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter);
gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f\n", this->filament_diameter);
+ gcode->stream->printf(";E retract length, feedrate, zlift length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f\n", this->retract_length, this->retract_feedrate*60.0F, this->retract_zlift_length, this->retract_zlift_feedrate);
+ gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f\n", this->retract_recover_length, this->retract_recover_feedrate*60.0F);
+
} else {
- gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter_setting, this->identifier);
+ gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter, this->identifier);
gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f P%d\n", this->filament_diameter, this->identifier);
+ gcode->stream->printf(";E retract length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f P%d\n", this->retract_length, this->retract_feedrate*60.0F, this->retract_zlift_length, this->retract_zlift_feedrate, this->identifier);
+ gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f P%d\n", this->retract_recover_length, this->retract_recover_feedrate*60.0F, this->identifier);
}
gcode->mark_as_taken();
- return;
+
+ } else if( gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 ) {
+ // Mcodes to pass along to on_gcode_execute
+ THEKERNEL->conveyor->append_gcode(gcode);
+ gcode->mark_as_taken();
}
- }
- // Gcodes to pass along to on_gcode_execute
- if( ( gcode->has_m && (gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 ) ) || ( gcode->has_g && gcode->g == 92 && gcode->has_letter('E') ) || ( gcode->has_g && ( gcode->g == 90 || gcode->g == 91 ) ) ) {
- THEKERNEL->conveyor->append_gcode(gcode);
- }
+ }else if(gcode->has_g) {
+ // G codes, NOTE some are ignored if not enabled
+ if( (gcode->g == 92 && gcode->has_letter('E')) || (gcode->g == 90 || gcode->g == 91) ) {
+ // Gcodes to pass along to on_gcode_execute
+ THEKERNEL->conveyor->append_gcode(gcode);
+ gcode->mark_as_taken();
- // Add to the queue for on_gcode_execute to process
- if( gcode->has_g && gcode->g < 4 && gcode->has_letter('E') && this->enabled ) {
- if( !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) {
+ }else if( this->enabled && gcode->g < 4 && gcode->has_letter('E') && !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) {
+ // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
THEKERNEL->conveyor->append_gcode(gcode);
+ THEKERNEL->conveyor->queue_head_block();
+ gcode->mark_as_taken();
+
+ }else if( this->enabled && (gcode->g == 10 || gcode->g == 11) ) { // firmware retract command
+ gcode->mark_as_taken();
+ // check we are in the correct state of retract or unretract
+ if(gcode->g == 10 && !retracted)
+ retracted= true;
+ else if(gcode->g == 11 && retracted)
+ retracted= false;
+ else
+ return; // ignore duplicates
+
+ // now we do a special hack to add zlift if needed, this should go in Robot but if it did the zlift would be executed before retract which is bad
+ // this way zlift will happen after retract, (or before for unretract) NOTE we call the robot->on_gcode_receive directly to avoid recursion
+ if(retract_zlift_length > 0 && gcode->g == 11) {
+ // reverse zlift happens before unretract
+ char buf[32];
+ int n= snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate);
+ string cmd(buf, n);
+ Gcode gc(cmd, &(StreamOutput::NullStream));
+ bool oldmode= THEKERNEL->robot->absolute_mode;
+ THEKERNEL->robot->absolute_mode= false; // needs to be relative mode
+ THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+ THEKERNEL->robot->absolute_mode= oldmode; // restore mode
+ }
+
// This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
+ THEKERNEL->conveyor->append_gcode(gcode);
THEKERNEL->conveyor->queue_head_block();
- }
- } else {
- // This is for follow move
+ if(retract_zlift_length > 0 && gcode->g == 10) {
+ char buf[32];
+ int n= snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate);
+ string cmd(buf, n);
+ Gcode gc(cmd, &(StreamOutput::NullStream));
+ bool oldmode= THEKERNEL->robot->absolute_mode;
+ THEKERNEL->robot->absolute_mode= false; // needs to be relative mode
+ THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+ THEKERNEL->robot->absolute_mode= oldmode; // restore mode
+ }
+ }
}
}
{
Gcode *gcode = static_cast<Gcode *>(argument);
- // Absolute/relative mode
+ // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
+ this->mode = OFF;
+
+ // Absolute/relative mode, globably modal affect all extruders whether enabled or not
if( gcode->has_m ) {
- if( gcode->m == 17 ) {
+ switch(gcode->m) {
+ case 17:
this->en_pin.set(0);
- }
- if( gcode->m == 18 ) {
+ break;
+ case 18:
this->en_pin.set(1);
- }
- if( gcode->m == 82 ) {
+ break;
+ case 82:
this->absolute_mode = true;
- }
- if( gcode->m == 83 ) {
+ break;
+ case 83:
this->absolute_mode = false;
- }
- if( gcode->m == 84 ) {
+ break;
+ case 84:
this->en_pin.set(1);
+ break;
}
+ return;
+
+ } else if( gcode->has_g && (gcode->g == 90 || gcode->g == 91) ) {
+ this->absolute_mode = (gcode->g == 90);
+ return;
}
- // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
- this->mode = OFF;
- if( gcode->has_g ) {
+ if( gcode->has_g && this->enabled ) {
// G92: Reset extruder position
- if( gcode->g == 92 && this->enabled ) {
+ if( gcode->g == 92 ) {
if( gcode->has_letter('E') ) {
this->current_position = gcode->get_value('E');
this->target_position = this->current_position;
this->unstepped_distance = 0;
}
- } else if (((gcode->g == 0) || (gcode->g == 1)) && this->enabled) {
+ } else if (gcode->g == 10) {
+ // FW retract command
+ feed_rate= retract_feedrate; // mm/sec
+ this->mode = SOLO;
+ this->travel_distance = -retract_length;
+ this->target_position += this->travel_distance;
+ this->en_pin.set(0);
+
+ } else if (gcode->g == 11) {
+ // un retract command
+ feed_rate= retract_recover_feedrate; // mm/sec
+ this->mode = SOLO;
+ this->travel_distance = (retract_length + retract_recover_length);
+ this->target_position += this->travel_distance;
+ this->en_pin.set(0);
+
+ } else if (gcode->g == 0 || gcode->g == 1) {
// Extrusion length from 'G' Gcode
if( gcode->has_letter('E' )) {
// Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position )
}
// If the robot is moving, we follow it's movement, otherwise, we move alone
- if( fabs(gcode->millimeters_of_travel) < 0.0001 ) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg
+ if( fabs(gcode->millimeters_of_travel) < 0.0001F ) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg
this->mode = SOLO;
this->travel_distance = relative_extrusion_distance;
} else {
// We move proportionally to the robot's movement
this->mode = FOLLOW;
- this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel;
+ this->travel_ratio = (relative_extrusion_distance * this->volumetric_multiplier) / gcode->millimeters_of_travel; // adjust for volumetric extrusion
// TODO: check resulting flowrate, limit robot speed if it exceeds max_speed
}
if (feed_rate > max_speed)
feed_rate = max_speed;
}
-
- } else if( gcode->g == 90 ) {
- this->absolute_mode = true;
-
- } else if( gcode->g == 91 ) {
- this->absolute_mode = false;
}
}
this->current_position += this->travel_distance ;
- int steps_to_step = abs(int(floor(this->steps_per_millimeter_setting * (this->travel_distance + this->unstepped_distance) )));
+ int steps_to_step = abs(int(floor(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) )));
if ( this->travel_distance > 0 ) {
- this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter_setting); //catch any overflow
+ this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); //catch any overflow
} else {
- this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter_setting); //catch any overflow
+ this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter); //catch any overflow
}
if( steps_to_step != 0 ) {
}
uint32_t current_rate = this->stepper_motor->get_steps_per_second();
- uint32_t target_rate = int(floor(this->feed_rate * this->steps_per_millimeter_setting)); // NOTE we use real steps here not the volumetric ones
+ uint32_t target_rate = int(floor(this->feed_rate * this->steps_per_millimeter));
if( current_rate < target_rate ) {
- uint32_t rate_increase = int(floor((this->acceleration / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * this->steps_per_millimeter_setting));
+ uint32_t rate_increase = int(floor((this->acceleration / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * this->steps_per_millimeter));
current_rate = min( target_rate, current_rate + rate_increase );
}
if( current_rate > target_rate ) {
return 0;
}
-
-void Extruder::update_steps_per_millimeter() {
- if(this->filament_diameter > 0.01) {
- this->steps_per_millimeter = this->steps_per_millimeter_setting / (powf(this->filament_diameter / 2, 2) * PI);
- } else {
- this->steps_per_millimeter = this->steps_per_millimeter_setting;
- }
-}
-
private:
void on_get_public_data(void* argument);
- void update_steps_per_millimeter();
Pin step_pin; // Step pin for the stepper driver
Pin dir_pin; // Dir pin for the stepper driver
float unstepped_distance; // overflow buffer for requested moves that are less than 1 step
Block* current_block; // Current block we are stepping, same as Stepper's one
- float steps_per_millimeter; // Steps to travel one millimeter
-
// kept together so they can be passed as public data
struct {
- float steps_per_millimeter_setting; // original steps to travel one millimeter as set in config, saved while in volumetric mode
- float filament_diameter; // filament diameter
+ float steps_per_millimeter; // Steps to travel one millimeter
+ float filament_diameter; // filament diameter
};
+ float volumetric_multiplier;
float feed_rate; //
float acceleration; //
float max_speed;
float travel_ratio;
float travel_distance;
+ // for firmware retract
+ float retract_feedrate;
+ float retract_length;
+ float retract_recover_feedrate;
+ float retract_recover_length;
+ float retract_zlift_length;
+ float retract_zlift_feedrate;
+
char mode; // extruder motion mode, OFF, SOLO, or FOLLOW
struct {
bool absolute_mode:1; // absolute/relative coordinate mode switch
bool paused:1;
bool single_config:1;
+ bool retracted:1;
};
StepperMotor* stepper_motor;
--- /dev/null
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "SCARAcal.h"
+
+#include "Kernel.h"
+#include "BaseSolution.h"
+#include "Config.h"
+#include "Robot.h"
+#include "StepperMotor.h"
+#include "StreamOutputPool.h"
+#include "Gcode.h"
+#include "Conveyor.h"
+#include "Stepper.h"
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "SerialMessage.h"
+#include "EndstopsPublicAccess.h"
+#include "PublicData.h"
+
+#define scaracal_checksum CHECKSUM("scaracal")
+#define enable_checksum CHECKSUM("enable")
+#define slow_feedrate_checksum CHECKSUM("slow_feedrate")
+
+#define X_AXIS 0
+#define Y_AXIS 1
+#define Z_AXIS 2
+
+#define STEPPER THEKERNEL->robot->actuators
+#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
+
+void SCARAcal::on_module_loaded()
+{
+ // if the module is disabled -> do nothing
+ if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) {
+ // as this module is not needed free up the resource
+ delete this;
+ return;
+ }
+
+ // load settings
+ this->on_config_reload(this);
+ // register event-handlers
+ register_for_event(ON_GCODE_RECEIVED);
+}
+
+void SCARAcal::on_config_reload(void *argument)
+{
+ this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec
+
+}
+
+
+// issue home command
+void SCARAcal::home()
+{
+ Gcode gc("G28", &(StreamOutput::NullStream));
+ THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
+}
+
+bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream)
+{
+ float t[3]{x, y, z};
+ bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t);
+
+ if (ok) {
+ stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
+ } else {
+ stream->printf("unable to set trim, is endstops enabled?\n");
+ }
+
+ return ok;
+}
+
+bool SCARAcal::get_trim(float& x, float& y, float& z)
+{
+ void *returned_data;
+ bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
+
+ if (ok) {
+ float *trim = static_cast<float *>(returned_data);
+ x= trim[0];
+ y= trim[1];
+ z= trim[2];
+ return true;
+ }
+ return false;
+}
+
+void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate)
+{
+ char cmd[64];
+ float actuator[3],
+ cartesian[3];
+
+ // Assign the actuator angles from input
+ actuator[0] = theta;
+ actuator[1] = psi;
+ actuator[2] = z;
+
+ // Calculate the physical position relating to the arm angles
+ THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, cartesian );
+
+ // Assemble Gcode to add onto the queue
+ snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec)
+
+ //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);
+
+ Gcode gc(cmd, &(StreamOutput::NullStream));
+ THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+}
+
+//A GCode has been received
+//See if the current Gcode line has some orders for us
+void SCARAcal::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+
+ if( gcode->has_m) {
+ switch( gcode->m ) {
+
+ case 114: { // Extra stuff for Morgan calibration
+ char buf[32];
+ float cartesian[3],
+ actuators[3];
+
+ THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+
+ int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f",
+ actuators[0],
+ actuators[1]); // display actuator angles Theta and Psi.
+ gcode->txt_after_ok.append(buf, n);
+ gcode->mark_as_taken();
+
+ }
+ return;
+
+ case 360: {
+ float target[2] = {0.0F, 120.0F},
+ S_trim[3];
+
+ this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
+
+ if(gcode->has_letter('P')) {
+ // Program the current position as target
+ float cartesian[3],
+ actuators[3],
+ S_delta[2],
+ S_trim[3];
+
+ THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+
+ S_delta[0] = actuators[0] - target[0];
+
+ set_trim(S_delta[0], S_trim[1], 0, gcode->stream);
+ } else {
+ set_trim(0, S_trim[1], 0, gcode->stream); // reset trim for calibration move
+ this->home(); // home
+ SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
+ }
+ gcode->mark_as_taken();
+ }
+ return;
+ case 361: {
+ float target[2] = {90.0F, 130.0F};
+ if(gcode->has_letter('P')) {
+ // Program the current position as target
+ float cartesian[3],
+ actuators[3];
+
+ THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+
+ STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
+ STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per *angle*
+ } else {
+ this->home(); // home - This time leave trims as adjusted.
+ SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
+ }
+ gcode->mark_as_taken();
+ }
+ return;
+ case 364: {
+ float target[2] = {45.0F, 135.0F},
+ S_trim[3];
+
+ this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
+
+ if(gcode->has_letter('P')) {
+ // Program the current position as target
+ float cartesian[3],
+ actuators[3],
+ S_delta[2];
+
+ THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles
+
+ S_delta[1] = actuators[1] - target[1]; // Find difference, and
+ set_trim(S_trim[0], S_delta[1], 0, gcode->stream); // set trim to reflect the difference
+ } else {
+ set_trim(S_trim[0], 0, 0, gcode->stream); // reset trim for calibration move
+ this->home(); // home
+ SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
+ }
+ gcode->mark_as_taken();
+ }
+ return;
+ }
+ }
+}
+
--- /dev/null
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef SCARACAL_H_
+#define SCARACAL_H_
+
+#include "Module.h"
+#include "Pin.h"
+
+class StepperMotor;
+class Gcode;
+class StreamOutput;
+
+class SCARAcal: public Module
+{
+
+public:
+ void on_module_loaded();
+ void on_config_reload(void *argument);
+
+ void on_gcode_received(void *argument);
+// void on_idle(void *argument);
+
+
+private:
+ void home();
+ bool set_trim(float x, float y, float z, StreamOutput *stream);
+ bool get_trim(float& x, float& y, float& z);
+
+ void SCARA_ang_move(float theta, float psi, float z, float feedrate);
+
+ float slow_rate;
+
+ struct {
+ bool is_scara:1;
+ };
+};
+
+#endif /* SCARACAL_H_ */
} else {
this->set_desired_temperature(v);
- if( gcode->m == this->set_and_wait_m_code) {
+ if( gcode->m == this->set_and_wait_m_code && !this->waiting) {
THEKERNEL->pauser->take();
this->waiting = true;
}
this->on_config_reload(this);
// register event-handlers
register_for_event(ON_GCODE_RECEIVED);
- register_for_event(ON_IDLE);
THEKERNEL->slow_ticker->attach( THEKERNEL->stepper->get_acceleration_ticks_per_second() , this, &ZProbe::acceleration_tick );
}
}
}
-void ZProbe::on_idle(void *argument)
-{
-}
-
// single probe and report amount moved
bool ZProbe::run_probe(int& steps, bool fast)
{
void on_module_loaded();
void on_config_reload(void *argument);
void on_gcode_received(void *argument);
- void on_idle(void *argument);
uint32_t acceleration_tick(uint32_t dummy);
-#include "libs/Kernel.h"
-#include "CurrentControl.h"
-#include "libs/nuts_bolts.h"
-#include "libs/utils.h"
-#include "ConfigValue.h"
-#include "libs/StreamOutput.h"
-
-#include "Gcode.h"
-#include "Config.h"
-#include "checksumm.h"
-
-// add new digipot chips here
-#include "mcp4451.h"
-#include "ad5206.h"
-
-#include <string>
-using namespace std;
-
-CurrentControl::CurrentControl(){
- digipot= NULL;
-}
-
-void CurrentControl::on_module_loaded(){
- if( !THEKERNEL->config->value( currentcontrol_module_enable_checksum )->by_default(false)->as_bool() ){
- // as this module is not needed free up the resource
- delete this;
- return;
- }
-
- // allocate digipot, if already allocated delete it first
- delete digipot;
-
- // see which chip to use
- int chip_checksum = get_checksum(THEKERNEL->config->value(digipotchip_checksum)->by_default("mcp4451")->as_string());
- if(chip_checksum == mcp4451_checksum) {
- digipot = new MCP4451();
- }else if(chip_checksum == ad5206_checksum) {
- digipot = new AD5206();
- }else { // need a default so use smoothie
- digipot = new MCP4451();
- }
-
- // Get configuration
- this->alpha_current = THEKERNEL->config->value(alpha_current_checksum )->by_default(0.8f)->as_number();
- this->beta_current = THEKERNEL->config->value(beta_current_checksum )->by_default(0.8f)->as_number();
- this->gamma_current = THEKERNEL->config->value(gamma_current_checksum )->by_default(0.8f)->as_number();
- this->delta_current = THEKERNEL->config->value(delta_current_checksum )->by_default(0.8f)->as_number();
- this->epsilon_current = THEKERNEL->config->value(epsilon_current_checksum)->by_default(-1)->as_number();
- this->zeta_current = THEKERNEL->config->value(zeta_current_checksum )->by_default(-1)->as_number();
- this->eta_current = THEKERNEL->config->value(eta_current_checksum )->by_default(-1)->as_number();
- this->theta_current = THEKERNEL->config->value(theta_current_checksum )->by_default(-1)->as_number();
-
- digipot->set_max_current( THEKERNEL->config->value(digipot_max_current )->by_default(2.0f)->as_number());
- digipot->set_factor( THEKERNEL->config->value(digipot_factor )->by_default(113.33f)->as_number());
-
- this->digipot->set_current(0, this->alpha_current);
- this->digipot->set_current(1, this->beta_current );
- this->digipot->set_current(2, this->gamma_current);
- this->digipot->set_current(3, this->delta_current);
- if(this->epsilon_current >= 0){
- this->digipot->set_current(4, this->epsilon_current);
- this->digipot->set_current(5, this->zeta_current );
- this->digipot->set_current(6, this->eta_current);
- this->digipot->set_current(7, this->theta_current);
- }
-
- this->original_delta_current= this->delta_current; // remember this to determine if we want to save on M500
-
- this->register_for_event(ON_GCODE_RECEIVED);
-}
-
-
-void CurrentControl::on_gcode_received(void *argument)
-{
- Gcode *gcode = static_cast<Gcode*>(argument);
- char alpha[8] = { 'X', 'Y', 'Z', 'E', 'A', 'B', 'C', 'D' };
- if (gcode->has_m)
- {
- if (gcode->m == 907)
- {
- int i;
- for (i = 0; i < 8; i++)
- {
- if (gcode->has_letter(alpha[i])){
- float c= gcode->get_value(alpha[i]);
- this->digipot->set_current(i, c);
- switch(i) {
- case 0: this->alpha_current= c; break;
- case 1: this->beta_current= c; break;
- case 2: this->gamma_current= c; break;
- case 3: this->delta_current= c; break;
- case 4: this->epsilon_current= c; break;
- case 5: this->zeta_current= c; break;
- case 6: this->eta_current= c; break;
- case 7: this->theta_current= c; break;
- }
- }
- gcode->stream->printf("%c:%3.1fA%c", alpha[i], this->digipot->get_current(i), (i == 7)?'\n':' ');
- }
-
- }else if(gcode->m == 500 || gcode->m == 503) {
- if(this->delta_current != this->original_delta_current) { // if not the same as loaded by config then save it
- gcode->stream->printf(";Extruder current:\nM907 E%1.5f\n", this->delta_current);
- }
- }
- }
-}
+#include "CurrentControl.h"
+#include "libs/Kernel.h"
+#include "libs/nuts_bolts.h"
+#include "libs/utils.h"
+#include "ConfigValue.h"
+#include "libs/StreamOutput.h"
+
+#include "Gcode.h"
+#include "Config.h"
+#include "checksumm.h"
+#include "DigipotBase.h"
+
+// add new digipot chips here
+#include "mcp4451.h"
+#include "ad5206.h"
+
+#include <string>
+using namespace std;
+
+#define alpha_current_checksum CHECKSUM("alpha_current")
+#define beta_current_checksum CHECKSUM("beta_current")
+#define gamma_current_checksum CHECKSUM("gamma_current")
+#define delta_current_checksum CHECKSUM("delta_current")
+#define epsilon_current_checksum CHECKSUM("epsilon_current")
+#define zeta_current_checksum CHECKSUM("zeta_current")
+#define eta_current_checksum CHECKSUM("eta_current")
+#define theta_current_checksum CHECKSUM("theta_current")
+#define currentcontrol_module_enable_checksum CHECKSUM("currentcontrol_module_enable")
+#define digipotchip_checksum CHECKSUM("digipotchip")
+#define digipot_max_current CHECKSUM("digipot_max_current")
+#define digipot_factor CHECKSUM("digipot_factor")
+
+#define mcp4451_checksum CHECKSUM("mcp4451")
+#define ad5206_checksum CHECKSUM("ad5206")
+
+CurrentControl::CurrentControl()
+{
+ digipot = NULL;
+}
+
+void CurrentControl::on_module_loaded()
+{
+ if( !THEKERNEL->config->value( currentcontrol_module_enable_checksum )->by_default(false)->as_bool() ) {
+ // as this module is not needed free up the resource
+ delete this;
+ return;
+ }
+
+ // allocate digipot, if already allocated delete it first
+ delete digipot;
+
+ // see which chip to use
+ int chip_checksum = get_checksum(THEKERNEL->config->value(digipotchip_checksum)->by_default("mcp4451")->as_string());
+ if(chip_checksum == mcp4451_checksum) {
+ digipot = new MCP4451();
+ } else if(chip_checksum == ad5206_checksum) {
+ digipot = new AD5206();
+ } else { // need a default so use smoothie
+ digipot = new MCP4451();
+ }
+
+ digipot->set_max_current( THEKERNEL->config->value(digipot_max_current )->by_default(2.0f)->as_number());
+ digipot->set_factor( THEKERNEL->config->value(digipot_factor )->by_default(113.33f)->as_number());
+
+ // Get configuration
+ this->digipot->set_current(0, THEKERNEL->config->value(alpha_current_checksum )->by_default(0.8f)->as_number());
+ this->digipot->set_current(1, THEKERNEL->config->value(beta_current_checksum )->by_default(0.8f)->as_number());
+ this->digipot->set_current(2, THEKERNEL->config->value(gamma_current_checksum )->by_default(0.8f)->as_number());
+ this->digipot->set_current(3, THEKERNEL->config->value(delta_current_checksum )->by_default(0.8f)->as_number());
+ this->digipot->set_current(4, THEKERNEL->config->value(epsilon_current_checksum)->by_default(-1)->as_number());
+ this->digipot->set_current(5, THEKERNEL->config->value(zeta_current_checksum )->by_default(-1)->as_number());
+ this->digipot->set_current(6, THEKERNEL->config->value(eta_current_checksum )->by_default(-1)->as_number());
+ this->digipot->set_current(7, THEKERNEL->config->value(theta_current_checksum )->by_default(-1)->as_number());
+
+
+ this->register_for_event(ON_GCODE_RECEIVED);
+}
+
+
+void CurrentControl::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode*>(argument);
+ char alpha[8] = { 'X', 'Y', 'Z', 'E', 'A', 'B', 'C', 'D' };
+ if (gcode->has_m) {
+ if (gcode->m == 907) {
+ for (int i = 0; i < 8; i++) {
+ if (gcode->has_letter(alpha[i])) {
+ float c = gcode->get_value(alpha[i]);
+ this->digipot->set_current(i, c);
+ }
+ }
+
+ } else if(gcode->m == 500 || gcode->m == 503) {
+ gcode->stream->printf(";Motor currents:\nM907 ");
+ for (int i = 0; i < 8; i++) {
+ float c = this->digipot->get_current(i);
+ if(c >= 0)
+ gcode->stream->printf("%c%1.5f ", alpha[i], c);
+ }
+ gcode->stream->printf("\n");
+ }
+ }
+}
-#ifndef CURRENTCONTROL_H
-#define CURRENTCONTROL_H
-
-#include "libs/Kernel.h"
-#include "libs/nuts_bolts.h"
-#include "libs/utils.h"
-#include "libs/Pin.h"
-#include "DigipotBase.h"
-
-#define alpha_current_checksum CHECKSUM("alpha_current")
-#define beta_current_checksum CHECKSUM("beta_current")
-#define gamma_current_checksum CHECKSUM("gamma_current")
-#define delta_current_checksum CHECKSUM("delta_current")
-#define epsilon_current_checksum CHECKSUM("epsilon_current")
-#define zeta_current_checksum CHECKSUM("zeta_current")
-#define eta_current_checksum CHECKSUM("eta_current")
-#define theta_current_checksum CHECKSUM("theta_current")
-#define currentcontrol_module_enable_checksum CHECKSUM("currentcontrol_module_enable")
-#define digipotchip_checksum CHECKSUM("digipotchip")
-#define digipot_max_current CHECKSUM("digipot_max_current")
-#define digipot_factor CHECKSUM("digipot_factor")
-
-#define mcp4451_checksum CHECKSUM("mcp4451")
-#define ad5206_checksum CHECKSUM("ad5206")
-
-class CurrentControl : public Module {
- public:
- CurrentControl();
- virtual ~CurrentControl() {};
-
- void on_module_loaded();
- void on_gcode_received(void *);
-
- private:
- float alpha_current;
- float beta_current;
- float gamma_current;
- float delta_current;
- float epsilon_current;
- float zeta_current;
- float eta_current;
- float theta_current;
- float original_delta_current;
-
- DigipotBase* digipot;
-
-};
-
-
-
-
-
-#endif
+#ifndef CURRENTCONTROL_H
+#define CURRENTCONTROL_H
+
+#include "Module.h"
+
+class DigipotBase;
+
+class CurrentControl : public Module {
+ public:
+ CurrentControl();
+ virtual ~CurrentControl() {};
+
+ void on_module_loaded();
+ void on_gcode_received(void *);
+
+ private:
+ DigipotBase* digipot;
+
+};
+
+
+
+
+
+#endif
#ifndef DIGIPOTBASE_H
#define DIGIPOTBASE_H
-#include "libs/Kernel.h"
-#include "libs/utils.h"
-#include <string>
-#include <math.h>
-
class DigipotBase {
public:
DigipotBase(){}
this->spi= new mbed::SPI(P0_9,P0_8,P0_7); //should be able to set those pins in config
cs.from_string("4.29")->as_output(); //this also should be configurable
cs.set(1);
+ for (int i = 0; i < 6; i++) currents[i] = -1;
}
void set_current( int channel, float current )
{
if(channel<6){
+ if(current < 0) {
+ currents[channel]= -1;
+ return;
+ }
current = min( max( current, 0.0L ), 2.0L );
char adresses[6] = { 0x05, 0x03, 0x01, 0x00, 0x02, 0x04 };
currents[channel] = current;
float get_current(int channel)
{
- return currents[channel];
+ if(channel < 6)
+ return currents[channel];
+ return -1;
}
private:
// I2C com
this->i2c = new mbed::I2C(p9, p10);
this->i2c->frequency(20000);
- for (int i = 0; i < 8; i++)
- currents[i] = 0.0;
+ for (int i = 0; i < 8; i++) currents[i] = -1;
}
~MCP4451(){
void set_current( int channel, float current )
{
+ if(current < 0) {
+ currents[channel]= -1;
+ return;
+ }
current = min( (float) max( current, 0.0f ), this->max_current );
currents[channel] = current;
char addr = 0x58;
uint32_t Panel::on_pause(uint32_t dummy)
{
- if (!paused) {
+ if (!THEKERNEL->pauser->paused()) {
THEKERNEL->pauser->take();
- paused = true;
} else {
THEKERNEL->pauser->release();
- paused = false;
}
return 0;
}
volatile bool refresh_flag;
volatile bool do_buttons;
volatile bool do_encoder;
- bool paused;
+
int idle_time;
bool start_up;
int encoder_click_resolution;
void PauseButton::on_module_loaded(){
this->button_state = true;
- this->play_state = true;
this->enable = THEKERNEL->config->value( pause_button_enable_checksum )->by_default(false)->as_bool();
this->button.from_string( THEKERNEL->config->value( pause_button_pin_checksum )->by_default("2.12")->as_string())->as_input();
}
//TODO: Make this use InterruptIn
-//Check the state of the button and act accordingly
+//Check the state of the button and act accordingly based on current pause state
uint32_t PauseButton::button_tick(uint32_t dummy){
if(!this->enable) return 0;
// If button changed
this->button_state = newstate;
// If button pressed
if( this->button_state ){
- if( this->play_state ){
- this->play_state = false;
- THEKERNEL->pauser->take();
- }else{
- this->play_state = true;
+ if( THEKERNEL->pauser->paused() ){
THEKERNEL->pauser->release();
+ }else{
+ THEKERNEL->pauser->take();
}
}
}
int checksum = get_checksum(shift_parameter(new_message.message));
if (checksum == freeze_command_checksum) {
- if( this->play_state ){
- this->play_state = false;
+ if( !THEKERNEL->pauser->paused() ){
THEKERNEL->pauser->take();
}
- }
- else if (checksum == unfreeze_command_checksum) {
- if( ! this->play_state ){
- this->play_state = true;
+
+ }else if (checksum == unfreeze_command_checksum) {
+ if( THEKERNEL->pauser->paused() ){
THEKERNEL->pauser->release();
}
}
struct {
bool enable:1;
bool button_state:1;
- bool play_state:1;
};
};