void error_detect_sensor_disconnected(int deviceReading, struct ErrorCounter* errCounter, char* deviceName) {
//Check if the last 6 counts has been the same (= something is wrong)
if (deviceReading == 1023) {
errCounter->counter++;
//errCounter->lastValue = deviceReading;
} else {
//errCounter->lastValue = deviceReading;
errCounter->counter = 0;
}
if (errCounter->counter == 6) {
emergency_stop(deviceName,"Disconnected");
}
}
/* ENQUEUES detected candy to bouded-buffer */
int buffer_enqueue(struct BoundedBuffer* bb, struct Candy candy) {
/* If the write pointer, plus one, equals the read pointer,
* then the buffer is full. */
if(((bb->write + 1) % bb->size) == bb->read)
emergency_stop("Buffer full:",bb->prettyname);//instead of return -1;
/* Buffer is initialized to contain a finit amount of candy.
* Therefore, we alter the value of the current candy pointed
* to by the write-head of the buffer. */
bb->buffer[bb->write] = candy;
/* Make the buffer circular */
bb->write= (bb->write + 1) % bb->size;
return 0;
}
void error_detect_is_dog_alive() {
if(systick_get_ms() - lastDogKickReceived > 200) { // > X, where X is the time in ms. that we allow the NXTs to NOT communicate.
emergency_stop("RS485 Error","Watchdog Timeout");
}
}
void process_gcode_command() {
uint32_t backup_f;
// convert relative to absolute
if (next_target.option_all_relative) {
next_target.target.X += startpoint.X;
next_target.target.Y += startpoint.Y;
next_target.target.Z += startpoint.Z;
}
// E relative movement.
// Matches Sprinter's behaviour as of March 2012.
if (next_target.option_all_relative || next_target.option_e_relative)
next_target.target.e_relative = 1;
else
next_target.target.e_relative = 0;
// implement axis limits
#ifdef X_MIN
if (next_target.target.X < X_MIN * 1000.)
next_target.target.X = X_MIN * 1000.;
#endif
#ifdef X_MAX
if (next_target.target.X > X_MAX * 1000.)
next_target.target.X = X_MAX * 1000.;
#endif
#ifdef Y_MIN
if (next_target.target.Y < Y_MIN * 1000.)
next_target.target.Y = Y_MIN * 1000.;
#endif
#ifdef Y_MAX
if (next_target.target.Y > Y_MAX * 1000.)
next_target.target.Y = Y_MAX * 1000.;
#endif
#ifdef Z_MIN
if (next_target.target.Z < Z_MIN * 1000.)
next_target.target.Z = Z_MIN * 1000.;
#endif
#ifdef Z_MAX
if (next_target.target.Z > Z_MAX * 1000.)
next_target.target.Z = Z_MAX * 1000.;
#endif
// The GCode documentation was taken from http://reprap.org/wiki/Gcode .
if (next_target.seen_T) {
//? --- T: Select Tool ---
//?
//? Example: T1
//?
//? Select extruder number 1 to build with. Extruder numbering starts at 0.
next_tool = next_target.T;
}
if (next_target.seen_G) {
uint8_t axisSelected = 0;
switch (next_target.G) {
case 0:
//? G0: Rapid Linear Motion
//?
//? Example: G0 X12
//?
//? In this case move rapidly to X = 12 mm. In fact, the RepRap firmware uses exactly the same code for rapid as it uses for controlled moves (see G1 below), as - for the RepRap machine - this is just as efficient as not doing so. (The distinction comes from some old machine tools that used to move faster if the axes were not driven in a straight line. For them G0 allowed any movement in space to get to the destination as fast as possible.)
//?
backup_f = next_target.target.F;
next_target.target.F = MAXIMUM_FEEDRATE_X * 2L;
enqueue(&next_target.target);
next_target.target.F = backup_f;
break;
case 1:
//? --- G1: Linear Motion at Feed Rate ---
//?
//? Example: G1 X90.6 Y13.8 E22.4
//?
//? Go in a straight line from the current (X, Y) point to the point (90.6, 13.8), extruding material as the move happens from the current extruded length to a length of 22.4 mm.
//?
enqueue(&next_target.target);
break;
// G2 - Arc Clockwise
// unimplemented
// G3 - Arc Counter-clockwise
// unimplemented
case 4:
//? --- G4: Dwell ---
//?
//? Example: G4 P200
//?
//? In this case sit still doing nothing for 200 milliseconds. During delays the state of the machine (for example the temperatures of its extruders) will still be preserved and controlled.
//?
queue_wait();
// delay
if (next_target.seen_P) {
for (;next_target.P > 0;next_target.P--) {
clock();
delay_ms(1);
}
}
break;
case 20:
//? --- G20: Set Units to Inches ---
//?
//? Example: G20
//?
//? Units from now on are in inches.
//?
next_target.option_inches = 1;
break;
case 21:
//? --- G21: Set Units to Millimeters ---
//?
//? Example: G21
//?
//? Units from now on are in millimeters. (This is the RepRap default.)
//?
next_target.option_inches = 0;
break;
case 30:
//? --- G30: Go home via point ---
//?
//? Undocumented.
enqueue(&next_target.target);
// no break here, G30 is move and then go home
case 28:
//? --- G28: Home ---
//?
//? Example: G28
//?
//? This causes the RepRap machine to move back to its X, Y and Z zero endstops. It does so accelerating, so as to get there fast. But when it arrives it backs off by 1 mm in each direction slowly, then moves back slowly to the stop. This ensures more accurate positioning.
//?
//? If you add coordinates, then just the axes with coordinates specified will be zeroed. Thus
//?
//? G28 X0 Y72.3
//?
//? will zero the X and Y axes, but not Z. The actual coordinate values are ignored.
//?
queue_wait();
if (next_target.seen_X) {
#if defined X_MIN_PIN
home_x_negative();
#elif defined X_MAX_PIN
home_x_positive();
#endif
axisSelected = 1;
}
if (next_target.seen_Y) {
#if defined Y_MIN_PIN
home_y_negative();
#elif defined Y_MAX_PIN
home_y_positive();
#endif
axisSelected = 1;
}
if (next_target.seen_Z) {
#if defined Z_MAX_PIN
home_z_positive();
#elif defined Z_MIN_PIN
home_z_negative();
#endif
axisSelected = 1;
}
// there's no point in moving E, as E has no endstops
if (!axisSelected) {
home();
}
break;
case 90:
//? --- G90: Set to Absolute Positioning ---
//?
//? Example: G90
//?
//? All coordinates from now on are absolute relative to the origin
//? of the machine. This is the RepRap default.
//?
//? If you ever want to switch back and forth between relative and
//? absolute movement keep in mind, X, Y and Z follow the machine's
//? coordinate system while E doesn't change it's position in the
//? coordinate system on relative movements.
//?
// No wait_queue() needed.
next_target.option_all_relative = 0;
break;
case 91:
//? --- G91: Set to Relative Positioning ---
//?
//? Example: G91
//?
//? All coordinates from now on are relative to the last position.
//?
// No wait_queue() needed.
next_target.option_all_relative = 1;
break;
case 92:
//? --- G92: Set Position ---
//?
//? Example: G92 X10 E90
//?
//? Allows programming of absolute zero point, by reseting the current position to the values specified. This would set the machine's X coordinate to 10, and the extrude coordinate to 90. No physical motion will occur.
//?
queue_wait();
if (next_target.seen_X) {
startpoint.X = next_target.target.X;
axisSelected = 1;
}
if (next_target.seen_Y) {
startpoint.Y = next_target.target.Y;
axisSelected = 1;
}
if (next_target.seen_Z) {
startpoint.Z = next_target.target.Z;
axisSelected = 1;
}
if (next_target.seen_E) {
startpoint.E = next_target.target.E;
axisSelected = 1;
}
if (axisSelected == 0) {
startpoint.X = next_target.target.X =
startpoint.Y = next_target.target.Y =
startpoint.Z = next_target.target.Z =
startpoint.E = next_target.target.E = 0;
}
dda_new_startpoint();
break;
case 161:
//? --- G161: Home negative ---
//?
//? Find the minimum limit of the specified axes by searching for the limit switch.
//?
if (next_target.seen_X)
home_x_negative();
if (next_target.seen_Y)
home_y_negative();
if (next_target.seen_Z)
home_z_negative();
break;
case 162:
//? --- G162: Home positive ---
//?
//? Find the maximum limit of the specified axes by searching for the limit switch.
//?
if (next_target.seen_X)
home_x_positive();
if (next_target.seen_Y)
home_y_positive();
if (next_target.seen_Z)
home_z_positive();
break;
// unknown gcode: spit an error
default:
sersendf_P(PSTR("E: Bad G-code %d"), next_target.G);
// newline is sent from gcode_parse after we return
return;
}
}
else if (next_target.seen_M) {
uint8_t i;
switch (next_target.M) {
case 0:
//? --- M0: machine stop ---
//?
//? Example: M0
//?
//? http://linuxcnc.org/handbook/RS274NGC_3/RS274NGC_33a.html#1002379
//? Unimplemented, especially the restart after the stop. Fall trough to M2.
//?
case 2:
case 18:
case 84: // For compatibility with slic3rs default end G-code.
//? --- M2: program end ---
//?
//? Example: M2
//?
//? http://linuxcnc.org/handbook/RS274NGC_3/RS274NGC_33a.html#1002379
//?
queue_wait();
for (i = 0; i < NUM_HEATERS; i++)
temp_set(i, 0);
power_off();
break;
case 112:
//? --- M112: Emergency Stop ---
//?
//? Example: M112
//?
//? Any moves in progress are immediately terminated, then RepRap shuts down. All motors and heaters are turned off.
//? It can be started again by pressing the reset button on the master microcontroller. See also M0.
//?
emergency_stop();
break;
case 6:
//? --- M6: tool change ---
//?
//? Undocumented.
tool = next_tool;
break;
case 82:
//? --- M82 - Set E codes absolute ---
//?
//? This is the default and overrides G90/G91.
//? M82/M83 is not documented in the RepRap wiki, behaviour
//? was taken from Sprinter as of March 2012.
//?
//? While E does relative movements, it doesn't change its
//? position in the coordinate system. See also comment on G90.
//?
// No wait_queue() needed.
next_target.option_e_relative = 0;
break;
case 83:
//? --- M83 - Set E codes relative ---
//?
//? Counterpart to M82.
//?
// No wait_queue() needed.
next_target.option_e_relative = 1;
break;
// M17- Enable/Power all stepper motors
case 17:
power_on();
break;
// M3/M101- extruder on
case 3:
case 101:
//? --- M101: extruder on ---
//?
//? Undocumented.
if (temp_achieved() == 0) {
enqueue(NULL);
}
#ifdef DC_EXTRUDER
heater_set(DC_EXTRUDER, DC_EXTRUDER_PWM);
#elif E_STARTSTOP_STEPS > 0
do {
// backup feedrate, move E very quickly then restore feedrate
backup_f = startpoint.F;
startpoint.F = MAXIMUM_FEEDRATE_E;
SpecialMoveE(E_STARTSTOP_STEPS, MAXIMUM_FEEDRATE_E);
startpoint.F = backup_f;
} while (0);
#endif
break;
// M102- extruder reverse
// M5/M103- extruder off
case 5:
case 103:
//? --- M103: extruder off ---
//?
//? Undocumented.
#ifdef DC_EXTRUDER
heater_set(DC_EXTRUDER, 0);
#elif E_STARTSTOP_STEPS > 0
do {
// backup feedrate, move E very quickly then restore feedrate
backup_f = startpoint.F;
startpoint.F = MAXIMUM_FEEDRATE_E;
SpecialMoveE(-E_STARTSTOP_STEPS, MAXIMUM_FEEDRATE_E);
startpoint.F = backup_f;
} while (0);
#endif
break;
case 104:
//? --- M104: Set Extruder Temperature (Fast) ---
//?
//? Example: M104 S190
//?
//? Set the temperature of the current extruder to 190<sup>o</sup>C
//? and return control to the host immediately (''i.e.'' before that
//? temperature has been reached by the extruder). For waiting, see M116.
//?
//? Teacup supports an optional P parameter as a zero-based temperature
//? sensor index to address (e.g. M104 P1 S100 will set the temperature
//? of the heater connected to the second temperature sensor rather
//? than the extruder temperature).
//?
if ( ! next_target.seen_S)
break;
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
// else use the first available device
#endif
temp_set(next_target.P, next_target.S);
break;
case 105:
//? --- M105: Get Temperature(s) ---
//?
//? Example: M105
//?
//? Request the temperature of the current extruder and the build base
//? in degrees Celsius. For example, the line sent to the host in
//? response to this command looks like
//?
//? <tt>ok T:201 B:117</tt>
//?
//? Teacup supports an optional P parameter as a zero-based temperature
//? sensor index to address.
//?
#ifdef ENFORCE_ORDER
queue_wait();
#endif
if ( ! next_target.seen_P)
next_target.P = TEMP_SENSOR_none;
temp_print(next_target.P);
break;
case 7:
case 106:
//? --- M106: Set Fan Speed / Set Device Power ---
//?
//? Example: M106 S120
//?
//? Control the cooling fan (if any).
//?
//? Teacup supports an optional P parameter as a zero-based heater
//? index to address. The heater index can differ from the temperature
//? sensor index, see config.h.
#ifdef ENFORCE_ORDER
// wait for all moves to complete
queue_wait();
#endif
#ifdef HEATER_FAN
if ( ! next_target.seen_P)
next_target.P = HEATER_FAN;
// else use the first available device
#endif
if ( ! next_target.seen_S)
break;
heater_set(next_target.P, next_target.S);
break;
case 110:
//? --- M110: Set Current Line Number ---
//?
//? Example: N123 M110
//?
//? Set the current line number to 123. Thus the expected next line after this command will be 124.
//? This is a no-op in Teacup.
//?
break;
#ifdef DEBUG
case 111:
//? --- M111: Set Debug Level ---
//?
//? Example: M111 S6
//?
//? Set the level of debugging information transmitted back to the host to level 6. The level is the OR of three bits:
//?
//? <Pre>
//? #define DEBUG_PID 1
//? #define DEBUG_DDA 2
//? #define DEBUG_POSITION 4
//? </pre>
//?
//? This command is only available in DEBUG builds of Teacup.
if ( ! next_target.seen_S)
break;
debug_flags = next_target.S;
break;
#endif
case 114:
//? --- M114: Get Current Position ---
//?
//? Example: M114
//?
//? This causes the RepRap machine to report its current X, Y, Z and E coordinates to the host.
//?
//? For example, the machine returns a string such as:
//?
//? <tt>ok C: X:0.00 Y:0.00 Z:0.00 E:0.00</tt>
//?
#ifdef ENFORCE_ORDER
// wait for all moves to complete
queue_wait();
#endif
update_current_position();
sersendf_P(PSTR("X:%lq,Y:%lq,Z:%lq,E:%lq,F:%lu"),
current_position.X, current_position.Y,
current_position.Z, current_position.E,
current_position.F);
#ifdef DEBUG
if (DEBUG_POSITION && (debug_flags & DEBUG_POSITION)) {
sersendf_P(PSTR(",c:%lu}\nEndpoint: X:%ld,Y:%ld,Z:%ld,E:%ld,F:%lu,c:%lu}"),
movebuffer[mb_tail].c, movebuffer[mb_tail].endpoint.X,
movebuffer[mb_tail].endpoint.Y, movebuffer[mb_tail].endpoint.Z,
movebuffer[mb_tail].endpoint.E, movebuffer[mb_tail].endpoint.F,
#ifdef ACCELERATION_REPRAP
movebuffer[mb_tail].end_c
#else
movebuffer[mb_tail].c
#endif
);
print_queue();
}
#endif /* DEBUG */
// newline is sent from gcode_parse after we return
break;
case 115:
//? --- M115: Get Firmware Version and Capabilities ---
//?
//? Example: M115
//?
//? Request the Firmware Version and Capabilities of the current microcontroller
//? The details are returned to the host computer as key:value pairs separated by spaces and terminated with a linefeed.
//?
//? sample data from firmware:
//? FIRMWARE_NAME:Teacup FIRMWARE_URL:http://github.com/triffid/Teacup_Firmware/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1 TEMP_SENSOR_COUNT:1 HEATER_COUNT:1
//?
sersendf_P(PSTR("FIRMWARE_NAME:Teacup FIRMWARE_URL:http://github.com/triffid/Teacup_Firmware/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:%d TEMP_SENSOR_COUNT:%d HEATER_COUNT:%d"), 1, NUM_TEMP_SENSORS, NUM_HEATERS);
// newline is sent from gcode_parse after we return
break;
case 116:
//? --- M116: Wait ---
//?
//? Example: M116
//?
//? Wait for temperatures and other slowly-changing variables to arrive at their set values.
enqueue(NULL);
break;
case 119:
//? --- M119: report endstop status ---
//? Report the current status of the endstops configured in the
//? firmware to the host.
power_on();
endstops_on();
delay_ms(10); // allow the signal to stabilize
{
const char* const open = PSTR("open ");
const char* const triggered = PSTR("triggered ");
#if defined(X_MIN_PIN)
sersendf_P(PSTR("x_min:"));
x_min() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if defined(X_MAX_PIN)
sersendf_P(PSTR("x_max:"));
x_max() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if defined(Y_MIN_PIN)
sersendf_P(PSTR("y_min:"));
y_min() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if defined(Y_MAX_PIN)
sersendf_P(PSTR("y_max:"));
y_max() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if defined(Z_MIN_PIN)
sersendf_P(PSTR("z_min:"));
z_min() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if defined(Z_MAX_PIN)
sersendf_P(PSTR("z_max:"));
z_max() ? sersendf_P(triggered) : sersendf_P(open);
#endif
#if ! (defined(X_MIN_PIN) || defined(X_MAX_PIN) || \
defined(Y_MIN_PIN) || defined(Y_MAX_PIN) || \
defined(Z_MIN_PIN) || defined(Z_MAX_PIN))
sersendf_P(PSTR("no endstops defined"));
#endif
}
endstops_off();
break;
#ifdef EECONFIG
case 130:
//? --- M130: heater P factor ---
//? Undocumented.
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
// else use the first available device
#endif
if (next_target.seen_S)
pid_set_p(next_target.P, next_target.S);
break;
case 131:
//? --- M131: heater I factor ---
//? Undocumented.
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
#endif
if (next_target.seen_S)
pid_set_i(next_target.P, next_target.S);
break;
case 132:
//? --- M132: heater D factor ---
//? Undocumented.
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
#endif
if (next_target.seen_S)
pid_set_d(next_target.P, next_target.S);
break;
case 133:
//? --- M133: heater I limit ---
//? Undocumented.
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
#endif
if (next_target.seen_S)
pid_set_i_limit(next_target.P, next_target.S);
break;
case 134:
//? --- M134: save PID settings to eeprom ---
//? Undocumented.
heater_save_settings();
break;
#endif /* EECONFIG */
#ifdef DEBUG
case 136:
//? --- M136: PRINT PID settings to host ---
//? Undocumented.
//? This comand is only available in DEBUG builds.
#ifdef HEATER_EXTRUDER
if ( ! next_target.seen_P)
next_target.P = HEATER_EXTRUDER;
heater_print(next_target.P);
#endif
break;
#endif
case 140:
//? --- M140: Set heated bed temperature ---
//? Undocumented.
#ifdef HEATER_BED
if ( ! next_target.seen_S)
break;
temp_set(HEATER_BED, next_target.S);
#endif
break;
#ifdef DEBUG
case 240:
//? --- M240: echo off ---
//? Disable echo.
//? This command is only available in DEBUG builds.
debug_flags &= ~DEBUG_ECHO;
serial_writestr_P(PSTR("Echo off"));
// newline is sent from gcode_parse after we return
break;
case 241:
//? --- M241: echo on ---
//? Enable echo.
//? This command is only available in DEBUG builds.
debug_flags |= DEBUG_ECHO;
serial_writestr_P(PSTR("Echo on"));
// newline is sent from gcode_parse after we return
break;
#endif /* DEBUG */
#ifdef PID_AUTOTUNE
case 303:
//? --- M303: PID autotune ---
//? Perform pid autotune procedure.
//? Hotend Usage: M303 S<temperature> C<cycles> Bed Usage: M303 E-1 C<cycles> S<temperature> Example: M303 C8 S175
{
uint32_t temp = 150*4;
int32_t e = 0;
uint32_t c = 8;
if (next_target.seen_E)
e = next_target.target.E;
if (e < 0)
temp = 70;
if (next_target.seen_S) temp = next_target.S;
if (next_target.seen_C) c = next_target.C;
PID_autotune(temp, e, c);
}
#endif
// unknown mcode: spit an error
default:
sersendf_P(PSTR("E: Bad M-code %d"), next_target.M);
// newline is sent from gcode_parse after we return
} // switch (next_target.M)
} // else if (next_target.seen_M)
} // process_gcode_command()
void read_handler(boost::system::error_code ec, std::size_t bytes_transferred)
{
if (!ec) {
ROS_INFO("Received %d bytes", (int)bytes_transferred);
lunabotics::proto::Telecommand tc;
if (!tc.ParseFromArray(buffer.data(), bytes_transferred)) {
ROS_ERROR("Failed to parse Telecommand object");
emergency_stop();
return;
}
lunabotics::Connection connectionMsg;
connectionMsg.port = tc.reply_port();
connectionMsg.ip = sock.remote_endpoint().address().to_string();
connPublisher.publish(connectionMsg);
switch(tc.type()) {
case lunabotics::proto::Telecommand::SET_AUTONOMY: {
ROS_INFO("%s autonomy", tc.autonomy_data().enabled() ? "Enabling" : "Disabling");
std_msgs::Bool autonomyMsg;
autonomyMsg.data = tc.autonomy_data().enabled();
autonomyPublisher.publish(autonomyMsg);
}
break;
case lunabotics::proto::Telecommand::STEERING_MODE: {
lunabotics::proto::SteeringModeType type = tc.steering_mode_data().type();
ROS_INFO("Switching control mode to %s", steeringModeToString(type).c_str());
lunabotics::SteeringMode controlModeMsg;
controlModeMsg.mode = type;
controlModeMsg.distance_accuracy = tc.steering_mode_data().position_accuracy();
controlModeMsg.angle_accuracy = tc.steering_mode_data().heading_accuracy();
controlModeMsg.linear_speed_limit = tc.steering_mode_data().max_linear_velocity();
controlModeMsg.bezier_segments = tc.steering_mode_data().bezier_curve_segments();
controlModePublisher.publish(controlModeMsg);
}
break;
case lunabotics::proto::Telecommand::TELEOPERATION: {
lunabotics::Teleoperation teleopMsg;
teleopMsg.forward = tc.teleoperation_data().forward();
teleopMsg.backward = tc.teleoperation_data().backward();
teleopMsg.left = tc.teleoperation_data().left();
teleopMsg.right = tc.teleoperation_data().right();
teleoperationPublisher.publish(teleopMsg);
}
break;
case lunabotics::proto::Telecommand::DEFINE_ROUTE: {
lunabotics::Goal goalMsg;
const lunabotics::proto::Telecommand::DefineRoute route = tc.define_route_data();
for (int i = 0; i < route.waypoints_size(); i++) {
const lunabotics::proto::Point waypoint = route.waypoints(i);
geometry_msgs::Point pt = lunabotics::geometry_msgs_Point_from_Point(lunabotics::CreatePoint(waypoint.x(), waypoint.y()));
goalMsg.waypoints.push_back(pt);
ROS_INFO("Navigation to (%.1f,%.1f)", pt.x, pt.y);
}
goalPublisher.publish(goalMsg);
}
break;
case lunabotics::proto::Telecommand::REQUEST_MAP: {
ROS_INFO("Receiving map request");
std_msgs::Empty emptyMsg;
mapRequestPublisher.publish(emptyMsg);
}
break;
case lunabotics::proto::Telecommand::ADJUST_PID: {
lunabotics::PID pidMsg;
pidMsg.p = tc.adjust_pid_data().p();
pidMsg.i = tc.adjust_pid_data().i();
pidMsg.d = tc.adjust_pid_data().d();
pidMsg.feedback_min_offset = tc.adjust_pid_data().feedback_min_offset();
pidMsg.feedback_multiplier = tc.adjust_pid_data().feedback_multiplier();
pidMsg.feedforward_min_offset = tc.adjust_pid_data().feedforward_min_offset();
pidMsg.feedforward_fraction = tc.adjust_pid_data().feedforward_fraction();
pidPublisher.publish(pidMsg);
}
break;
case lunabotics::proto::Telecommand::ADJUST_WHEELS: {
switch (tc.all_wheel_control_data().all_wheel_type()) {
case lunabotics::proto::AllWheelControl::EXPLICIT: {
lunabotics::proto::AllWheelState::Wheels driving = tc.all_wheel_control_data().explicit_data().driving();
lunabotics::proto::AllWheelState::Wheels steering = tc.all_wheel_control_data().explicit_data().steering();
float left_front = steering.left_front();
float right_front = steering.right_front();
float left_rear = steering.left_rear();
float right_rear = steering.right_rear();
lunabotics::validateAngles(left_front, right_front, left_rear, right_rear);
lunabotics::AllWheelState msg;
msg.driving.left_front = driving.left_front();
msg.driving.right_front = driving.right_front();
msg.driving.left_rear = driving.left_rear();
msg.driving.right_rear = driving.right_rear();
msg.steering.left_front = left_front;
msg.steering.right_front = right_front;
msg.steering.left_rear = left_rear;
msg.steering.right_rear = right_rear;
allWheelPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::PREDEFINED: {
lunabotics::AllWheelCommon msg;
msg.predefined_cmd = tc.all_wheel_control_data().predefined_data().command();
msg.wheel_velocity = DEFAULT_WHEEL_VELOCITY;
allWheelCommonPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::ICR: {
lunabotics::ICRControl msg;
msg.ICR.x = tc.all_wheel_control_data().icr_data().icr().x();
msg.ICR.y = tc.all_wheel_control_data().icr_data().icr().y();
msg.velocity = tc.all_wheel_control_data().icr_data().velocity();
ICRPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::CRAB: {
lunabotics::CrabControl msg;
msg.heading = tc.all_wheel_control_data().crab_data().heading();
msg.velocity = tc.all_wheel_control_data().crab_data().velocity();
CrabPublisher.publish(msg);
}
break;
default:
break;
}
}
break;
default:
break;
}
}
else if (ec.value() == boost::asio::error::eof) {
ROS_INFO("Connection closed by client");
}
else {
ROS_WARN("Failed to read data. Error code %s", ec.message().c_str());
}
sock.shutdown(boost::asio::ip::tcp::socket::shutdown_both, ec);
sock.close();
acceptor.async_accept(sock, accept_handler);
}
void CommandeGen::emerg_stop(){
m_status = emergency_stop() ;
}
/// called every 10ms from clock.c - check all temp sensors that are ready for checking
void temp_sensor_tick() {
temp_sensor_t i = 0;
for (; i < NUM_TEMP_SENSORS; i++) {
if (temp_sensors_runtime[i].next_read_time) {
temp_sensors_runtime[i].next_read_time--;
}
else {
uint16_t temp = 0;
#ifdef TEMP_MAX31855
uint32_t value = 0;
struct max31855_plat_data *max31855;
#endif
//time to deal with this temp sensor
switch(temp_sensors[i].temp_type) {
#ifdef TEMP_MAX31855
case TT_MAX31855:
#ifdef __arm__
max31855 = (struct max31855_plat_data*)temp_sensors[i].plat_data;
spiAcquireBus(max31855->bus);
spiStart(max31855->bus, max31855->config);
spiSelect(max31855->bus);
spiReceive(max31855->bus, 4, &value);
spiUnselect(max31855->bus);
spiReleaseBus(max31855->bus);
value = SWAP_UINT32(value);
temp_sensors_runtime[i].temp_flags = 0;
if (value & (1 << 16)) {
#ifdef HALT_ON_TERMOCOUPLE_FAILURE
emergency_stop();
#endif
}
else {
temp = value >> 18;
if (temp & (1 << 13))
temp = 0;
}
#else
temp = 0;
#endif
break;
#endif
#ifdef TEMP_MAX6675
case TT_MAX6675:
#ifdef PRR
PRR &= ~MASK(PRSPI);
#elif defined PRR0
PRR0 &= ~MASK(PRSPI);
#endif
SPCR = MASK(MSTR) | MASK(SPE) | MASK(SPR0);
// enable TT_MAX6675
WRITE(SS, 0);
// No delay required, see
// https://github.com/triffid/Teacup_Firmware/issues/22
// read MSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp = SPDR;
temp <<= 8;
// read LSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp |= SPDR;
// disable TT_MAX6675
WRITE(SS, 1);
temp_sensors_runtime[i].temp_flags = 0;
if ((temp & 0x8002) == 0) {
// got "device id"
temp_sensors_runtime[i].temp_flags |= PRESENT;
if (temp & 4) {
// thermocouple open
temp_sensors_runtime[i].temp_flags |= TCOPEN;
}
else {
temp = temp >> 3;
}
}
// this number depends on how frequently temp_sensor_tick is called. the MAX6675 can give a reading every 0.22s, so set this to about 250ms
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_MAX6675 */
#ifdef TEMP_THERMISTOR
case TT_THERMISTOR:
do {
uint8_t j, table_num;
//Read current temperature
temp = analog_read(i);
// for thermistors the thermistor table number is in the additional field
table_num = temp_sensors[i].additional;
//Calculate real temperature based on lookup table
for (j = 1; j < NUMTEMPS; j++) {
if (pgm_read_word(&(temptable[table_num][j][0])) > temp) {
// Thermistor table is already in 14.2 fixed point
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("pin:%d Raw ADC:%d table entry: %d"),temp_sensors[i].temp_pin,temp,j);
#endif
// Linear interpolating temperature value
// y = ((x - x₀)y₁ + (x₁-x)y₀ ) / (x₁ - x₀)
// y = temp
// x = ADC reading
// x₀= temptable[j-1][0]
// x₁= temptable[j][0]
// y₀= temptable[j-1][1]
// y₁= temptable[j][1]
// y =
// Wikipedia's example linear interpolation formula.
temp = (
// ((x - x₀)y₁
((uint32_t)temp - pgm_read_word(&(temptable[table_num][j-1][0]))) * pgm_read_word(&(temptable[table_num][j][1]))
// +
+
// (x₁-x)
(pgm_read_word(&(temptable[table_num][j][0])) - (uint32_t)temp)
// y₀ )
* pgm_read_word(&(temptable[table_num][j-1][1])))
// /
/
// (x₁ - x₀)
(pgm_read_word(&(temptable[table_num][j][0])) - pgm_read_word(&(temptable[table_num][j-1][0])));
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" temp:%d.%d"),temp/4,(temp%4)*25);
#endif
break;
}
}
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" Sensor:%d\n"),i);
#endif
//Clamp for overflows
if (j == NUMTEMPS)
temp = temptable[table_num][NUMTEMPS-1][1];
temp_sensors_runtime[i].next_read_time = 0;
} while (0);
break;
#endif /* TEMP_THERMISTOR */
#ifdef TEMP_AD595
case TT_AD595:
temp = analog_read(i);
// convert
// >>8 instead of >>10 because internal temp is stored as 14.2 fixed point
temp = (temp * 500L) >> 8;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_AD595 */
#ifdef TEMP_PT100
case TT_PT100:
#warning TODO: PT100 code
break
#endif /* TEMP_PT100 */
#ifdef TEMP_INTERCOM
case TT_INTERCOM:
temp = read_temperature(temp_sensors[i].temp_pin);
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_INTERCOM */
#ifdef TEMP_DUMMY
case TT_DUMMY:
temp = temp_sensors_runtime[i].last_read_temp;
if (temp_sensors_runtime[i].target_temp > temp)
temp++;
else if (temp_sensors_runtime[i].target_temp < temp)
temp--;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_DUMMY */
default: /* prevent compiler warning */
break;
}
/* Exponentially Weighted Moving Average alpha constant for smoothing
noisy sensors. Instrument Engineer's Handbook, 4th ed, Vol 2 p126
says values of 0.05 to 0.1 for TEMP_EWMA are typical. */
#ifndef TEMP_EWMA
#define TEMP_EWMA 1.0
#endif
#define EWMA_SCALE 1024L
#define EWMA_ALPHA ((long) (TEMP_EWMA * EWMA_SCALE))
temp_sensors_runtime[i].last_read_temp = (uint16_t) ((EWMA_ALPHA * temp +
(EWMA_SCALE-EWMA_ALPHA) * temp_sensors_runtime[i].last_read_temp
) / EWMA_SCALE);
}
if (labs((int16_t)(temp_sensors_runtime[i].last_read_temp - temp_sensors_runtime[i].target_temp)) < (TEMP_HYSTERESIS*4)) {
if (temp_sensors_runtime[i].temp_residency < (TEMP_RESIDENCY_TIME*120))
temp_sensors_runtime[i].temp_residency++;
}
else {
// Deal with flakey sensors which occasionally report a wrong value
// by setting residency back, but not entirely to zero.
if (temp_sensors_runtime[i].temp_residency > 10)
temp_sensors_runtime[i].temp_residency -= 10;
else
temp_sensors_runtime[i].temp_residency = 0;
}
if (temp_sensors[i].heater < NUM_HEATERS) {
heater_tick(temp_sensors[i].heater, temp_sensors[i].temp_type, temp_sensors_runtime[i].last_read_temp, temp_sensors_runtime[i].target_temp);
}
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("DU temp: {%d %d %d.%d}"), i,
temp_sensors_runtime[i].last_read_temp,
temp_sensors_runtime[i].last_read_temp / 4,
(temp_sensors_runtime[i].last_read_temp & 0x03) * 25);
}
RudderMotor::~RudderMotor()
{
emergency_stop();
can_close();
}
//
// Top level order handler
//
void process_command()
{
if (is_stopped && !stopped_is_acknowledged && order_code != ORDER_RESUME)
{
send_stopped_response();
return;
}
// we allow a sequence of write configuration commands
// to be set as configured as a group. For instance,
// writing the following sequence:
// device.stepper.0.enable_pin=48
// device.stepper.0.enable_invert=1
// ...
// will only result in the stepper configuration being applied when all
// attributes have been applied.
if (firmware_configuration_change_made || !final_firmware_configuration_update_done)
{
if (!final_firmware_configuration_update_done
&& (order_code == ORDER_SET_HEATER_TARGET_TEMP
|| order_code == ORDER_SET_OUTPUT_SWITCH_STATE
|| order_code == ORDER_SET_PWM_OUTPUT_STATE
|| order_code == ORDER_SET_OUTPUT_TONE
|| order_code == ORDER_ACTIVATE_STEPPER_CONTROL
|| order_code == ORDER_ENABLE_DISABLE_STEPPERS
|| order_code == ORDER_QUEUE_COMMAND_BLOCKS))
{
// assume that once devices are activated then initial firmware
// configuration is complete and EEPROM state should be updated
firmware_configuration_change_made = false;
final_firmware_configuration_update_done = true;
update_firmware_configuration(true);
}
else if (firmware_configuration_change_made
&& order_code != ORDER_WRITE_FIRMWARE_CONFIG_VALUE)
{
// update individual device states once firmware configuration
// block is done
firmware_configuration_change_made = false;
update_firmware_configuration(false);
}
}
switch (order_code)
{
case ORDER_RESET:
emergency_stop(PARAM_STOPPED_CAUSE_USER_REQUEST);
die();
break;
case ORDER_RESUME:
handle_resume_order();
break;
case ORDER_REQUEST_INFORMATION:
handle_request_information_order();
break;
case ORDER_DEVICE_COUNT:
handle_device_count_order();
break;
case ORDER_DEVICE_NAME:
handle_device_name_order();
break;
case ORDER_DEVICE_STATUS:
handle_device_status_order();
break;
case ORDER_REQUEST_TEMPERATURE_READING:
handle_request_temperature_reading_order();
break;
case ORDER_GET_HEATER_CONFIGURATION:
handle_get_heater_configuration_order();
break;
case ORDER_CONFIGURE_HEATER:
handle_configure_heater_order();
break;
case ORDER_SET_HEATER_TARGET_TEMP:
handle_set_heater_target_temperature_order();
break;
case ORDER_GET_INPUT_SWITCH_STATE:
handle_get_input_switch_state_order();
break;
case ORDER_SET_OUTPUT_SWITCH_STATE:
handle_set_output_switch_state_order();
break;
case ORDER_SET_PWM_OUTPUT_STATE:
handle_set_pwm_output_state_order();
break;
case ORDER_SET_OUTPUT_TONE:
handle_set_output_tone_order();
break;
case ORDER_WRITE_FIRMWARE_CONFIG_VALUE:
firmware_configuration_change_made = true;
handle_write_firmware_configuration_value_order();
break;
case ORDER_READ_FIRMWARE_CONFIG_VALUE:
// note: get_command() already makes the end of the command (ie. name) null-terminated
handle_firmware_configuration_request((const char *)¶meter_value[0], 0);
break;
case ORDER_TRAVERSE_FIRMWARE_CONFIG:
// note: get_command() already makes the end of the command (ie. name) null-terminated
handle_firmware_configuration_traversal((const char *)¶meter_value[0]);
break;
case ORDER_GET_FIRMWARE_CONFIG_PROPERTIES:
// note: get_command() already makes the end of the command (ie. name) null-terminated
handle_firmware_configuration_value_properties((const char *)¶meter_value[0]);
break;
case ORDER_EMERGENCY_STOP:
emergency_stop(PARAM_STOPPED_CAUSE_USER_REQUEST);
send_OK_response();
break;
case ORDER_ACTIVATE_STEPPER_CONTROL:
handle_activate_stepper_control_order();
break;
case ORDER_CONFIGURE_ENDSTOPS:
handle_configure_endstops_order();
break;
case ORDER_ENABLE_DISABLE_STEPPERS:
handle_enable_disable_steppers_order();
break;
case ORDER_ENABLE_DISABLE_ENDSTOPS:
handle_enable_disable_endstops_order();
break;
case ORDER_QUEUE_COMMAND_BLOCKS:
enqueue_command();
break;
case ORDER_CLEAR_COMMAND_QUEUE:
handle_clear_command_queue_order();
break;
case ORDER_CONFIGURE_AXIS_MOVEMENT_RATES:
handle_configure_axis_movement_rates_order();
break;
case ORDER_CONFIGURE_UNDERRUN_PARAMS:
handle_configure_underrun_params_order();
break;
default:
send_app_error_response(PARAM_APP_ERROR_TYPE_UNKNOWN_ORDER, 0);
break;
}
}
void loop()
{
// quickly scan through baudrates until we receive a valid packet within timeout period
#if 0 // the baud rate change still isn't working for some reason.
if (autodetect_baudrates_index != 0xFF)
{
if (millis() - first_rcvd_time > MAX_FRAME_COMPLETION_DELAY_MS * 1.5) // make sure its not a multiple of 100ms
{
PSERIAL.end();
PSERIAL.flush();
autodetect_baudrates_index += 1;
if (autodetect_baudrates_index >= NUM_ARRAY_ELEMENTS(autodetect_baudrates))
autodetect_baudrates_index = 0;
PSERIAL.begin(pgm_read_dword(&autodetect_baudrates[autodetect_baudrates_index]));
recv_buf_len = 0;
first_rcvd_time = millis();
}
}
#endif
if (get_command())
{
autodetect_baudrates_index = 0xFF;
last_order_time = millis();
is_host_active = true;
order_code = recv_buf[PM_ORDER_BYTE_OFFSET];
control_byte = recv_buf[PM_CONTROL_BYTE_OFFSET];
parameter_length = recv_buf[PM_LENGTH_BYTE_OFFSET]-2;
#if TRACE_ORDER
DEBUGPGM("\nOrder(");
DEBUG_F(order_code, HEX);
DEBUGPGM(", plen=");
DEBUG_F(parameter_length, DEC);
DEBUGPGM(", cb=");
DEBUG_F(control_byte, HEX);
DEBUGPGM("):");
for (uint8_t i = 0; i < parameter_length; i++)
{
DEBUG_CH(' ');
DEBUG_F(recv_buf[i], HEX);
}
DEBUG_EOL();
#endif
// does this match the sequence number of the last reply
if ((control_byte & CONTROL_BYTE_SEQUENCE_NUMBER_MASK) ==
(reply_control_byte & CONTROL_BYTE_SEQUENCE_NUMBER_MASK)
&& (control_byte & CONTROL_BYTE_ORDER_HOST_RESET_BIT) == 0
&& reply_control_byte != 0xFF)
{
if (!reply_started)
{
// resend last response
reply_started = true;
generate_response_send();
}
else
{
// this is an unexpected error case (matching sequence number but nothing to send)
generate_response_transport_error_start(PARAM_FRAME_RECEIPT_ERROR_TYPE_UNABLE_TO_ACCEPT,
control_byte);
generate_response_msg_addPGM(PMSG(MSG_ERR_NO_RESPONSE_TO_SEND));
generate_response_send();
}
}
else
{
reply_sent = false;
process_command();
if (!reply_sent)
{
send_app_error_response(PARAM_APP_ERROR_TYPE_FIRMWARE_ERROR,
PMSG(MSG_ERR_NO_RESPONSE_GENERATED));
}
}
recv_buf_len = 0;
}
// Idle loop activities
// Check if heaters need to be updated?
if (temp_meas_ready)
{
Device_TemperatureSensor::UpdateTemperatureSensors();
Device_Heater::UpdateHeaters();
}
// Now check low-priority stuff
uint32_t now = millis();
if (now - last_idle_check > 1000) // checked every second
{
#if !DEBUG_DISABLE_HOST_TIMEOUT
// have we heard from the host within the timeout?
if (now - last_order_time > (HOST_TIMEOUT_SECS * 1000) && is_host_active)
{
is_host_active = false;
if (!is_stopped)
{
emergency_stop(PARAM_STOPPED_CAUSE_HOST_TIMEOUT);
}
}
#endif
#if TRACE_MOVEMENT
print_movement_ISR_state();
#endif
last_idle_check = now;
}
}
SailMotor::~SailMotor()
{
rtx_message("SailMotor-Destructor has been called ... ");
emergency_stop();
}
