void SerialHandlerConfigState::handle() {
// LOGLN("Checking in status configuring");
// First trim spaces and tabs.
byte start;
for (start = 0; start < data->pos; start++) {
if (!(data->readBuffer[start] == 0x20 || data->readBuffer[start] == 0x9)) {
break;
}
}
ASSERT_MIN_LENGTH(start + 2)
// Check if we are ending the configuration.
if (data->pos >= (start + 3) && data->readBuffer[start] == '/'
&& data->readBuffer[start + 1] == 'C'
&& data->readBuffer[start + 2] == 'A') {
SerialConfigEncoder::transmitter.LastPartSend();
// LOGLN("Finished configuration.");
serialHandler->setHandler(new SerialHandlerIdleState(serialHandler));
return;
}
for (byte i = 0; i < data->encoderSize; i++) {
// See if any of the
if (memcmp(data->readBuffer + start, data->encoders[i].type, 4) == 0) {
// Cool, we have the matching encoder.
data->encoders[i].encoder->EncodeSerial2Bin(data->readBuffer + start + 5,
data->pos - start - 5);
// LOGLN("Handled by Encoder !");
// Done
return;
}
}
// If we reach this part of the code, no encoder was found.
ERRORLN("No Encoder found :");
ERRORLN((char*)(data->readBuffer + start));
}
void SerialHandler::RegisterEncoder(const byte* type,
SerialConfigEncoder* encoder) {
if (data.encoderSize == 5) {
ERRORLN("Only space for 5 encoders is reserved. Add more space in the SerialHandler.");
return;
} else {
data.encoders[data.encoderSize].type = type;
data.encoders[data.encoderSize++].encoder = encoder;
}
}
void SerialHandler::Try() {
if (Serial.available()) {
data.readBuffer[data.pos] = Serial.read();
if (data.readBuffer[data.pos] == (byte) 0x0D) {
// Handle the command.
if (state)
state->handle();
data.pos = 0;
} else {
if ((unsigned char) data.pos < (SERIAL_BUFFER_SIZE - 1)) {
data.pos++;
} else {
ERRORLN("BUFFER FULL");
data.pos = 0;
}
}
}
}
void emergency_stop(uint8_t new_stopped_cause, uint8_t new_stopped_type
/* = PARAM_STOPPED_TYPE_ONE_TIME_OR_CLEARED */)
{
ERROR("Stopping: cause="); // TODO improve with error reason /Language-ify
ERRORLN(new_stopped_cause);
uint8_t i;
is_stopped = true;
stopped_is_acknowledged = false;
stopped_cause = new_stopped_cause;
stopped_type = new_stopped_type;
for (i=0; i<Device_Stepper::GetNumDevices(); i++)
{
Device_Stepper::WriteEnableState(i, false);
}
for (i=0; i<Device_Heater::GetNumDevices(); i++)
{
if (Device_Heater::IsInUse(i))
{
Device_Heater::SetTargetTemperature(i, 0);
digitalWrite(Device_Heater::GetHeaterPin(i), LOW); // forces device to turn off immediately even with soft PWM.
}
}
for (i=0; i<Device_OutputSwitch::GetNumDevices(); i++)
{
Device_OutputSwitch::WriteState(i, OUTPUT_SWITCH_STATE_DISABLED);
}
for (i=0; i<Device_PwmOutput::GetNumDevices(); i++)
{
if (Device_PwmOutput::IsInUse(i))
{
Device_PwmOutput::WriteState(i, 0);
digitalWrite(Device_PwmOutput::GetPin(i), LOW); // forces device to turn off immediately even with soft PWM.
}
}
}
void Device_Heater::UpdateHeaters()
{
HeaterInfo *heater_info = heater_info_array;
for (uint8_t i=0; i<num_heaters; i++)
{
float target_temp = heater_info->target_temp;
if (target_temp != SENSOR_TEMPERATURE_INVALID)
{
// Check if temperature is within the correct range
float current_temp = Device_TemperatureSensor::ReadCurrentTemperature(heater_info->temp_sensor);
if (current_temp > heater_info->max_temp || current_temp == SENSOR_TEMPERATURE_INVALID)
{
// TODO handle heater error.
if (current_temp == SENSOR_TEMPERATURE_INVALID)
ERROR("Heater Read Error: ");
else
ERROR("Heater overtemp Error: ");
ERRORLN((int)i);
heater_info->target_temp = SENSOR_TEMPERATURE_INVALID;
SetHeaterPower(heater_info, 0);
}
else if (heater_info->control_mode == HEATER_CONTROL_MODE_BANG_BANG)
{
// Bang Bang Mode
if (heater_info->is_heating)
{
if (current_temp > target_temp + heater_info->control_info.bangbang.hysteresis)
{
SetHeaterPower(heater_info, 0);
}
}
else
{
if (current_temp < target_temp - heater_info->control_info.bangbang.hysteresis)
{
SetHeaterPower(heater_info, heater_info->power_on_level);
}
}
}
else if (heater_info->control_mode == HEATER_CONTROL_MODE_PID)
{
// PID mode
PidInfo *pid_info = heater_info->control_info.pid;
uint8_t pid_power;
float pid_error = target_temp - current_temp;
if (pid_error > pid_info->functional_range)
{
pid_power = heater_info->power_on_level;
pid_info->state_iState = 0.0; // reset PID state
}
else if(pid_error < -(pid_info->functional_range))
{
pid_power = 0;
pid_info->state_iState = 0.0; // reset PID state
}
else
{
// update and constrain iState
pid_info->state_iState += pid_error;
if (pid_info->state_iState > pid_info->iState_max)
pid_info->state_iState = pid_info->iState_max;
else if (pid_info->state_iState < 0.0)
pid_info->state_iState = 0.0;
pid_info->state_dTerm = ((current_temp - pid_info->last_temp) * pid_info->KdK2)
+ (pid_info->advanced_K1 * pid_info->state_dTerm);
float pTerm = pid_info->Kp * pid_error;
float iTerm = pid_info->Ki * pid_info->state_iState;
float pid_output = pTerm + iTerm - pid_info->state_dTerm;
if (pid_output >= (float)pid_info->advanced_max_pid_power_level)
pid_power = pid_info->advanced_max_pid_power_level;
else if (pid_output <= 0.0)
pid_power = 0;
else
pid_power = (uint8_t)pid_output;
#ifdef PID_DEBUG
DEBUG(" PIDDEBUG ");
DEBUG(e);
DEBUG(": Input ");
DEBUG(current_temp);
DEBUG(" Output ");
DEBUG(pid_output);
DEBUG(" pTerm ");
DEBUG(pTerm);
DEBUG(" iTerm ");
DEBUG(iTerm);
DEBUG(" dTerm ");
DEBUG(pid_info->state_dTerm);
#endif //PID_DEBUG
}
pid_info->last_temp = current_temp;
SetHeaterPower(heater_info, pid_power);
}
}
heater_info += 1;
}
}
