task main() {
//Set up precision speed control (which, incidentally, uses PID to do it :D)
nMotorPIDSpeedCtrl[left] = mtrSpeedReg;
nMotorPIDSpeedCtrl[right] = mtrSpeedReg;
//Initialize PIDs.
PID leftPID,rightPID,accelPID;
initPID(leftPID,3.5,0,0);
initPID(rightPID,3.5,0,0);
initPID(accelPID,2,0,0);
INTR velIntr;
initIntr(velIntr);
//Tuning tips: http://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops
//Also, double PID loops: http://forum.arduino.cc/index.php?topic=197688.0
//PID for physical position, not just rotational? (need accel)
//Swag: https://www.youtube.com/watch?v=n_6p-1J551Y
//Maybe we should try a unisphere balancing robot :) https://www.youtube.com/watch?v=bI06lujiD7E
//Stands up and initiates gyro stuff.
initSweg();
//waitForStart();
//Initialize steering, raise arms... let's start!!
StartTask(steer);
servo[rearFlipper] = REAR_LIFT_RAISED;
servo[frontFlipper] = FRONT_LIFT_RAISED;
EndTimeSlice();
while(true) {
int ax,ay,az;
HTACreadAllAxes(accel, ax, ay, az);
float yvel = integrate(velIntr,ay);
if(abs(forwardsAngle) > 50) {//If it fell over, reset and redo.
reset(leftPID);
reset(rightPID);
initSweg();
servo[rearFlipper] = REAR_LIFT_RAISED;
servo[frontFlipper] = FRONT_LIFT_RAISED;
}
//Separate left and right PIDs to allow steering by NeutralAngle adjustment.
motor[left] = updatePID(leftPID, leftNeutral - forwardsAngle);
motor[right] = updatePID(rightPID, rightNeutral - forwardsAngle);
motor[left] = -motor[right];
wait1Msec(5);
}
}
asynStatus ReadASCII::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
//Checks for updates to the index and on/off of PID lookup
int function = pasynUser->reason;
asynStatus status = asynSuccess;
const char *paramName;
const char* functionName = "writeInt32";
int LUTOn;
/* Set the parameter in the parameter library. */
status = (asynStatus)setIntegerParam(function, value);
if (function == P_Index) {
//check lookup on
getIntegerParam(P_LookUpOn, &LUTOn);
if (LUTOn)
{
//update all column floats
setDoubleParam(P_SPOut, pSP_[value]);
setDoubleParam(P_P, pP_[value]);
setDoubleParam(P_I, pI_[value]);
setDoubleParam(P_D, pD_[value]);
setDoubleParam(P_MaxHeat, pMaxHeat_[value]);
}
}else if (function == P_LookUpOn) {
//reload file if bad
if (true == fileBad)
{
status = readFileBasedOnParameters();
}
//file may now be good - retry
if (false == fileBad)
{
//uses the current temperature to find PID values
if (value)
{
double curTemp;
getDoubleParam(P_CurTemp, &curTemp);
updatePID(getSPInd(curTemp));
}
}
}
/* Do callbacks so higher layers see any changes */
status = (asynStatus)callParamCallbacks();
if (status)
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
"%s:%s: status=%d, function=%d, name=%s, value=%d",
driverName, functionName, status, function, paramName, value);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d, name=%s, value=%d\n",
driverName, functionName, function, paramName, value);
return status;
}
//////////////////////////////////////////////////////////////////////////////
/////////////////////////// processAltitudeHold //////////////////////////////
//////////////////////////////////////////////////////////////////////////////
void processAltitudeHold(void)
{
// ****************************** Altitude Adjust *************************
// Thanks to Honk for his work with altitude hold
// http://aeroquad.com/showthread.php?792-Problems-with-BMP085-I2C-barometer
// Thanks to Sherbakov for his work in Z Axis dampening
// http://aeroquad.com/showthread.php?359-Stable-flight-logic...&p=10325&viewfull=1#post10325
#ifdef AltitudeHold
if (altitudeHold == ON) {
throttleAdjust = updatePID(holdAltitude, altitude.getData(), &PID[ALTITUDE]);
//throttleAdjust = constrain((holdAltitude - altitude.getData()) * PID[ALTITUDE].P, minThrottleAdjust, maxThrottleAdjust);
throttleAdjust = constrain(throttleAdjust, minThrottleAdjust, maxThrottleAdjust);
if (abs(holdThrottle - receiver.getData(THROTTLE)) > PANICSTICK_MOVEMENT) {
altitudeHold = ALTPANIC; // too rapid of stick movement so PANIC out of ALTHOLD
} else {
if (receiver.getData(THROTTLE) > (holdThrottle + ALTBUMP)) { // AKA changed to use holdThrottle + ALTBUMP - (was MAXCHECK) above 1900
holdAltitude += 0.01;
}
if (receiver.getData(THROTTLE) < (holdThrottle - ALTBUMP)) { // AKA change to use holdThorrle - ALTBUMP - (was MINCHECK) below 1100
holdAltitude -= 0.01;
}
}
}
else {
// Altitude hold is off, get throttle from receiver
holdThrottle = receiver.getData(THROTTLE);
throttleAdjust = autoDescent; // autoDescent is lowered from BatteryMonitor.h during battery alarm
}
// holdThrottle set in FlightCommand.pde if altitude hold is on
throttle = holdThrottle + throttleAdjust; // holdThrottle is also adjust by BatteryMonitor.h during battery alarm
#else
// If altitude hold not enabled in AeroQuad.pde, get throttle from receiver
throttle = receiver.getData(THROTTLE) + autoDescent; //autoDescent is lowered from BatteryMonitor.h while battery critical, otherwise kept 0
#endif
}
void Config_RetrieveSettings()
{
int i=EEPROM_OFFSET;
char stored_ver[4];
char ver[4]=EEPROM_VERSION;
EEPROM_READ_VAR(i,stored_ver); //read stored version
// SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]");
if (strncmp(ver,stored_ver,3) == 0)
{
// version number match
EEPROM_READ_VAR(i,axis_steps_per_unit);
EEPROM_READ_VAR(i,max_feedrate);
EEPROM_READ_VAR(i,max_acceleration_units_per_sq_second);
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
EEPROM_READ_VAR(i,acceleration);
EEPROM_READ_VAR(i,retract_acceleration);
EEPROM_READ_VAR(i,minimumfeedrate);
EEPROM_READ_VAR(i,mintravelfeedrate);
EEPROM_READ_VAR(i,minsegmenttime);
EEPROM_READ_VAR(i,max_xy_jerk);
EEPROM_READ_VAR(i,max_z_jerk);
EEPROM_READ_VAR(i,max_e_jerk);
EEPROM_READ_VAR(i,add_homeing);
#ifndef ULTIPANEL
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed;
int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
#endif
EEPROM_READ_VAR(i,plaPreheatHotendTemp);
EEPROM_READ_VAR(i,plaPreheatHPBTemp);
EEPROM_READ_VAR(i,plaPreheatFanSpeed);
EEPROM_READ_VAR(i,absPreheatHotendTemp);
EEPROM_READ_VAR(i,absPreheatHPBTemp);
EEPROM_READ_VAR(i,absPreheatFanSpeed);
EEPROM_READ_VAR(i,zprobe_zoffset);
#ifndef PIDTEMP
float Kp,Ki,Kd;
#endif
// do not need to scale PID values as the values in EEPROM are already scaled
EEPROM_READ_VAR(i,Kp);
EEPROM_READ_VAR(i,Ki);
EEPROM_READ_VAR(i,Kd);
int lcd_contrast;
EEPROM_READ_VAR(i,lcd_contrast);
// Call updatePID (similar to when we have processed M301)
updatePID();
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Stored settings retrieved");
}
else
{
Config_ResetDefault();
}
#ifdef EEPROM_CHITCHAT
Config_PrintSettings();
#endif
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
long tmp4[]=DEFAULT_DIGIPOT_MOTOR_CURRENT;
for (short i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
digipot_motor_current[i]=tmp4[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef DELTA
endstop_adj[0] = endstop_adj[1] = endstop_adj[2] = 0;
#endif
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif
#ifdef DOGLCD
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
st_init();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
zprobe_offset=DEFAULT_Z_PROBE_OFFSET_FROM_EXTRUDER;
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
// Callback for after editing PID i value
// grab the pid i value out of the temp variable; scale it; then update the PID driver
void copy_and_scalePID_i()
{
#ifdef PIDTEMP
Ki = scalePID_i(raw_Ki);
updatePID();
#endif
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
for (short i=0; i<4; i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
float tmp_motor_current_setting[]=DEFAULT_PWM_MOTOR_CURRENT;
motor_current_setting[0] = tmp_motor_current_setting[0];
motor_current_setting[1] = tmp_motor_current_setting[1];
motor_current_setting[2] = tmp_motor_current_setting[2];
#ifdef ENABLE_ULTILCD2
led_brightness_level = 100;
led_mode = LED_MODE_ALWAYS_ON;
#endif
retract_length = 4.5;
retract_feedrate = 25 * 60;
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
// Callback for after editing PID d value
// grab the pid d value out of the temp variable; scale it; then update the PID driver
void copy_and_scalePID_d()
{
#ifdef PIDTEMP
Kd = scalePID_d(raw_Kd);
updatePID();
#endif
}
void Config_Postprocess() {
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
// Call updatePID (similar to when we have processed M301)
#ifdef PIDTEMP
updatePID();
#endif
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
short i;
for (i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef ENABLE_AUTO_BED_LEVELING
bed_level_probe_offset[0] = X_PROBE_OFFSET_FROM_EXTRUDER_DEFAULT;
bed_level_probe_offset[1] = Y_PROBE_OFFSET_FROM_EXTRUDER_DEFAULT;
bed_level_probe_offset[2] = Z_PROBE_OFFSET_FROM_EXTRUDER_DEFAULT;
#endif
#ifdef DOGLCD
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
base_min_pos[0] = X_MIN_POS_DEFAULT;
base_min_pos[1] = Y_MIN_POS_DEFAULT;
base_min_pos[2] = Z_MIN_POS_DEFAULT;
base_max_pos[0] = X_MAX_POS_DEFAULT;
base_max_pos[1] = Y_MAX_POS_DEFAULT;
base_max_pos[2] = Z_MAX_POS_DEFAULT;
ECHO_STRING("Hardcoded Default Settings Loaded");
}
ControlAlgorithm::ControlAlgorithm(QObject *parent) :
QObject(parent)
{
//TODO: point to already created kiteColorTracker instead of making new one
// kiteColorTracker = new KiteColorTracker(this);
imageProcessingWindow = new ImageProcessing();
kiteColorTracker = imageProcessingWindow->getColorTracker();
kiteColorTracker->setSampleRate(1);
// call update whenever a new position computed by kiteColorTracker
connect(kiteColorTracker,SIGNAL(dataUpdated()),this,SLOT(update()));
// initialize start and end points for left/right paths
//set quadrant parameters
OUTER_GRID_OFFSET_X=50;
OUTER_GRID_OFFSET_Y=25;
POWER_ZONE_X=100;
POWER_ZONE_Y=100;
initGrid();
// initialize PID controller
Kp = 1.0;
Ki = 0.0;
Kd = 0.0;
interval = 0.01; // in seconds
pid = new PID(Kp,Ki,Kd,interval);
pid->setMode(1.0); // 1 Automatic, 0 Manual
pid->setSetPoint(0.0); // setpoint will always be 0 degrees relative to current heading
pid->setInputLimits(-45.0,45.0);
pid->setOutputLimits(0.0,30.0); // turning values
pid->setProcessValue(10.0); // initialize input to be 0 so no error
// timer for pid loop
// timer will be started by a user input that we are now tracking the kite
pidTimer = new QTimer;
pidTimer->setInterval(interval*1000); // convert from s to ms
connect(pidTimer,SIGNAL(timeout()),this,SLOT(updatePID()));
//pidTimer->start();
autoPilotOn = false;
}
void ReadASCII::checkLookUp (double newSP, double oldSP)
{
//Checks if the SP has crossed a threshold in the file by comparing their indices
int newInd, oldInd;
newInd = getSPInd(newSP);
oldInd = getSPInd(oldSP);
if (newInd != oldInd)
{
updatePID(newInd);
}
}
/**
* Post-process after Retrieve or Reset
*/
void Config_Postprocess() {
// steps per s2 needs to be updated to agree with units per s2
planner.reset_acceleration_rates();
#if MECH(DELTA)
set_delta_constants();
#endif
#if ENABLED(PIDTEMP)
updatePID();
#endif
calculate_volumetric_multipliers();
}
void calculateFlightError(void)
{
if (flightMode == ACRO) {
motors.setMotorAxisCommand(ROLL, updatePID(receiver.getSIData(ROLL), gyro.getData(ROLL), &PID[ROLL]));
motors.setMotorAxisCommand(PITCH, updatePID(receiver.getSIData(PITCH), -gyro.getData(PITCH), &PID[PITCH]));
}
else {
float rollAttitudeCmd = updatePID((receiver.getData(ROLL) - receiver.getZero(ROLL)) * ATTITUDE_SCALING, flightAngle->getData(ROLL), &PID[LEVELROLL]);
float pitchAttitudeCmd = updatePID((receiver.getData(PITCH) - receiver.getZero(PITCH)) * ATTITUDE_SCALING, -flightAngle->getData(PITCH), &PID[LEVELPITCH]);
motors.setMotorAxisCommand(ROLL, updatePID(rollAttitudeCmd, gyro.getData(ROLL), &PID[LEVELGYROROLL]));
motors.setMotorAxisCommand(PITCH, updatePID(pitchAttitudeCmd, -gyro.getData(PITCH), &PID[LEVELGYROPITCH]));
}
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
for (short i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
for (short i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
e1_steps_per_unit=DEFAULT_E1_STEPS_PER_UNIT;
// Extruder offset
//for(short i=0; i<2; i++)
//{
//float e_offset_x[] = EXTRUDER_OFFSET_X;
//float e_offset_y[] = EXTRUDER_OFFSET_Y;
//extruder_offset[0][i]=e_offset_x[i];
//extruder_offset[1][i]=e_offset_y[i];
//}
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef DELTA
endstop_adj[0] = endstop_adj[1] = endstop_adj[2] = 0;
#endif
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
hipsPreheatHotendTemp = HIPS_PREHEAT_HOTEND_TEMP;
hipsPreheatHPBTemp = HIPS_PREHEAT_HPB_TEMP;
hipsPreheatFanSpeed = HIPS_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
bridgePreheatHotendTemp = BRIDGE_PREHEAT_HOTEND_TEMP;
bridgePreheatHPBTemp = BRIDGE_PREHEAT_HPB_TEMP;
bridgePreheatFanSpeed = BRIDGE_PREHEAT_FAN_SPEED;
pctpePreheatHotendTemp = PCTPE_PREHEAT_HOTEND_TEMP;
pctpePreheatHPBTemp = PCTPE_PREHEAT_HPB_TEMP;
pctpePreheatFanSpeed = PCTPE_PREHEAT_FAN_SPEED;
alloy_910PreheatHotendTemp = ALLOY_910_PREHEAT_HOTEND_TEMP;
alloy_910PreheatHPBTemp = ALLOY_910_PREHEAT_HPB_TEMP;
alloy_910PreheatFanSpeed = ALLOY_910_PREHEAT_FAN_SPEED;
//~ bambooPreheatHotendTemp = BAMBOO_PREHEAT_HOTEND_TEMP;
//~ bambooPreheatHPBTemp = BAMBOO_PREHEAT_HPB_TEMP;
//~ bambooPreheatFanSpeed = BAMBOO_PREHEAT_FAN_SPEED;
n_ventPreheatHotendTemp = N_VENT_PREHEAT_HOTEND_TEMP;
n_ventPreheatHPBTemp = N_VENT_PREHEAT_HPB_TEMP;
n_ventPreheatFanSpeed = N_VENT_PREHEAT_FAN_SPEED;
laybrickPreheatHotendTemp = LAYBRICK_PREHEAT_HOTEND_TEMP;
laybrickPreheatHPBTemp = LAYBRICK_PREHEAT_HPB_TEMP;
laybrickPreheatFanSpeed = LAYBRICK_PREHEAT_FAN_SPEED;
laywoodPreheatHotendTemp = LAYWOOD_PREHEAT_HOTEND_TEMP;
laywoodPreheatHPBTemp = LAYWOOD_PREHEAT_HPB_TEMP;
laywoodPreheatFanSpeed = LAYWOOD_PREHEAT_FAN_SPEED;
polycarbonatePreheatHotendTemp = POLYCARBONATE_PREHEAT_HOTEND_TEMP;
polycarbonatePreheatHPBTemp = POLYCARBONATE_PREHEAT_HPB_TEMP;
polycarbonatePreheatFanSpeed = POLYCARBONATE_PREHEAT_FAN_SPEED;
tglasePreheatHotendTemp = TGLASE_PREHEAT_HOTEND_TEMP;
tglasePreheatHPBTemp = TGLASE_PREHEAT_HPB_TEMP;
tglasePreheatFanSpeed = TGLASE_PREHEAT_FAN_SPEED;
#endif
#ifdef ENABLE_AUTO_BED_LEVELING
zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
#endif
#ifdef DOGLCD
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
void computeMotorCommands(float dt)
{
float rollCmdDiv2, pitchCmdDiv2, yawCmdDiv2;
float cosRollCmdDiv2, sinRollCmdDiv2;
float cosPitchCmdDiv2, sinPitchCmdDiv2;
float cosYawCmdDiv2, sinYawCmdDiv2;
float qRef[4];
float qMeasConjugate[4];
float qError[4];
float normR;
float qError0Squared, qError1Squared, qError2Squared, qError3Squared;
float axisError[3];
holdIntegrators = false;
///////////////////////////////////
rollCmdDiv2 = pointingCmd[ROLL ] / 2.0f;
pitchCmdDiv2 = pointingCmd[PITCH] / 2.0f;
yawCmdDiv2 = pointingCmd[YAW ] / 2.0f;
cosRollCmdDiv2 = cosf(rollCmdDiv2);
sinRollCmdDiv2 = sinf(rollCmdDiv2);
cosPitchCmdDiv2 = cosf(pitchCmdDiv2);
sinPitchCmdDiv2 = sinf(pitchCmdDiv2);
cosYawCmdDiv2 = cosf(yawCmdDiv2);
sinYawCmdDiv2 = sinf(yawCmdDiv2);
qRef[0] = ( cosRollCmdDiv2 * cosPitchCmdDiv2 * cosYawCmdDiv2 ) +
( sinRollCmdDiv2 * sinPitchCmdDiv2 * sinYawCmdDiv2 );
qRef[1] = ( sinRollCmdDiv2 * cosPitchCmdDiv2 * cosYawCmdDiv2 ) -
( cosRollCmdDiv2 * sinPitchCmdDiv2 * sinYawCmdDiv2 );
qRef[2] = ( cosRollCmdDiv2 * sinPitchCmdDiv2 * cosYawCmdDiv2 ) +
( sinRollCmdDiv2 * cosPitchCmdDiv2 * sinYawCmdDiv2 );
qRef[3] = ( cosRollCmdDiv2 * cosPitchCmdDiv2 * sinYawCmdDiv2 ) -
( sinRollCmdDiv2 * sinPitchCmdDiv2 * cosYawCmdDiv2 );
qMeasConjugate[0] = qMeas[0];
qMeasConjugate[1] = -qMeas[1];
qMeasConjugate[2] = -qMeas[2];
qMeasConjugate[3] = -qMeas[3];
qError[0] = qRef[0] * qMeasConjugate[0] - qRef[1] * qMeasConjugate[1] -
qRef[2] * qMeasConjugate[2] - qRef[3] * qMeasConjugate[3];
qError[1] = qRef[0] * qMeasConjugate[1] + qRef[1] * qMeasConjugate[0] +
qRef[2] * qMeasConjugate[3] - qRef[3] * qMeasConjugate[2];
qError[2] = qRef[0] * qMeasConjugate[2] - qRef[1] * qMeasConjugate[3] +
qRef[2] * qMeasConjugate[0] + qRef[3] * qMeasConjugate[1];
qError[3] = qRef[0] * qMeasConjugate[3] + qRef[1] * qMeasConjugate[2] -
qRef[2] * qMeasConjugate[1] + qRef[3] * qMeasConjugate[0];
// normalize quaternion
normR = 1.0f / sqrt(qError[0] * qError[0] + qError[1] * qError[1] + qError[2] * qError[2] + qError[3] * qError[3]);
qError[0] *= normR;
qError[1] *= normR;
qError[2] *= normR;
qError[3] *= normR;
qError0Squared = qError[0] * qError[0];
qError1Squared = qError[1] * qError[1];
qError2Squared = qError[2] * qError[2];
qError3Squared = qError[3] * qError[3];
axisError[ROLL ] = atan2f(2.0f * (qError[0] * qError[1] + qError[2] * qError[3]),
qError0Squared - qError1Squared - qError2Squared + qError3Squared);
axisError[PITCH] = asinf(2.0f * (qError[0] * qError[2] - qError[3] * qError[1]));
axisError[YAW ] = atan2f(2.0f * (qError[0] * qError[3] + qError[1] * qError[2]),
qError0Squared + qError1Squared - qError2Squared - qError3Squared);
///////////////////////////////////
if (eepromConfig.rollEnabled == true)
{
if (eepromConfig.pidController == true)
{
motorCmd[ROLL] = updatePID(pointingCmd[ROLL] * mechanical2electricalDegrees[ROLL],
sensors.evvgcCFAttitude500Hz[ROLL] * mechanical2electricalDegrees[ROLL],
dt, holdIntegrators, &eepromConfig.PID[ROLL_PID]);
}
else
{
motorCmd[ROLL] = updatePDF(pointingCmd[ROLL] * mechanical2electricalDegrees[ROLL],
sensors.evvgcCFAttitude500Hz[ROLL] * mechanical2electricalDegrees[ROLL],
dt, holdIntegrators, &eepromConfig.PDF[ROLL_PDF]);
}
outputRate[ROLL] = motorCmd[ROLL] - motorCmdPrev[ROLL];
if (outputRate[ROLL] > eepromConfig.rateLimit)
motorCmd[ROLL] = motorCmdPrev[ROLL] + eepromConfig.rateLimit;
if (outputRate[ROLL] < -eepromConfig.rateLimit)
motorCmd[ROLL] = motorCmdPrev[ROLL] - eepromConfig.rateLimit;
motorCmdPrev[ROLL] = motorCmd[ROLL];
setRollMotor(motorCmd[ROLL], (int)eepromConfig.rollPower);
}
///////////////////////////////////
if (eepromConfig.pitchEnabled == true)
{
if (eepromConfig.pidController == true)
{
motorCmd[PITCH] = updatePID(pointingCmd[PITCH] * mechanical2electricalDegrees[PITCH],
sensors.evvgcCFAttitude500Hz[PITCH] * mechanical2electricalDegrees[PITCH],
dt, holdIntegrators, &eepromConfig.PID[PITCH_PID]);
}
else
{
motorCmd[PITCH] = updatePDF(pointingCmd[PITCH] * mechanical2electricalDegrees[PITCH],
sensors.evvgcCFAttitude500Hz[PITCH] * mechanical2electricalDegrees[PITCH],
dt, holdIntegrators, &eepromConfig.PDF[PITCH_PDF]);
}
outputRate[PITCH] = motorCmd[PITCH] - motorCmdPrev[PITCH];
if (outputRate[PITCH] > eepromConfig.rateLimit)
motorCmd[PITCH] = motorCmdPrev[PITCH] + eepromConfig.rateLimit;
if (outputRate[PITCH] < -eepromConfig.rateLimit)
motorCmd[PITCH] = motorCmdPrev[PITCH] - eepromConfig.rateLimit;
motorCmdPrev[PITCH] = motorCmd[PITCH];
setPitchMotor(motorCmd[PITCH], (int)eepromConfig.pitchPower);
}
///////////////////////////////////
if (eepromConfig.yawEnabled == true)
{
if (eepromConfig.pidController == true)
{
motorCmd[YAW] = updatePID(pointingCmd[YAW] * mechanical2electricalDegrees[YAW],
sensors.evvgcCFAttitude500Hz[YAW] * mechanical2electricalDegrees[YAW],
dt, holdIntegrators, &eepromConfig.PID[YAW_PID]);
}
else
{
motorCmd[YAW] = updatePDF(pointingCmd[YAW] * mechanical2electricalDegrees[YAW],
sensors.evvgcCFAttitude500Hz[YAW] * mechanical2electricalDegrees[YAW],
dt, holdIntegrators, &eepromConfig.PDF[YAW_PDF]);
}
outputRate[YAW] = motorCmd[YAW] - motorCmdPrev[YAW];
if (outputRate[YAW] > eepromConfig.rateLimit)
motorCmd[YAW] = motorCmdPrev[YAW] + eepromConfig.rateLimit;
if (outputRate[YAW] < -eepromConfig.rateLimit)
motorCmd[YAW] = motorCmdPrev[YAW] - eepromConfig.rateLimit;
motorCmdPrev[YAW] = motorCmd[YAW];
setYawMotor(motorCmd[YAW], (int)eepromConfig.yawPower);
}
///////////////////////////////////
}
void Config_ResetDefault() {
float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[] = DEFAULT_MAX_FEEDRATE;
long tmp3[] = DEFAULT_MAX_ACCELERATION;
for (uint8_t i = 0; i < NUM_AXIS; i++) {
axis_steps_per_unit[i] = tmp1[i];
max_feedrate[i] = tmp2[i];
max_acceleration_units_per_sq_second[i] = tmp3[i];
#if ENABLED(SCARA)
if (i < COUNT(axis_scaling))
axis_scaling[i] = 1;
#endif
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration = DEFAULT_ACCELERATION;
retract_acceleration = DEFAULT_RETRACT_ACCELERATION;
travel_acceleration = DEFAULT_TRAVEL_ACCELERATION;
minimumfeedrate = DEFAULT_MINIMUMFEEDRATE;
minsegmenttime = DEFAULT_MINSEGMENTTIME;
mintravelfeedrate = DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk = DEFAULT_XYJERK;
max_z_jerk = DEFAULT_ZJERK;
max_e_jerk = DEFAULT_EJERK;
home_offset[X_AXIS] = home_offset[Y_AXIS] = home_offset[Z_AXIS] = 0;
#if ENABLED(MESH_BED_LEVELING)
mbl.active = 0;
#endif
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER;
#endif
#if ENABLED(DELTA)
endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
recalc_delta_settings(delta_radius, delta_diagonal_rod);
#elif ENABLED(Z_DUAL_ENDSTOPS)
z_endstop_adj = 0;
#endif
#if ENABLED(ULTIPANEL)
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif
#if ENABLED(HAS_LCD_CONTRAST)
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#if ENABLED(PIDTEMP)
#if ENABLED(PID_PARAMS_PER_EXTRUDER)
for (int e = 0; e < EXTRUDERS; e++)
#else
int e = 0; UNUSED(e); // only need to write once
#endif
{
PID_PARAM(Kp, e) = DEFAULT_Kp;
PID_PARAM(Ki, e) = scalePID_i(DEFAULT_Ki);
PID_PARAM(Kd, e) = scalePID_d(DEFAULT_Kd);
#if ENABLED(PID_ADD_EXTRUSION_RATE)
PID_PARAM(Kc, e) = DEFAULT_Kc;
#endif
}
#if ENABLED(PID_ADD_EXTRUSION_RATE)
lpq_len = 20; // default last-position-queue size
#endif
// call updatePID (similar to when we have processed M301)
updatePID();
#endif // PIDTEMP
#if ENABLED(PIDTEMPBED)
bedKp = DEFAULT_bedKp;
bedKi = scalePID_i(DEFAULT_bedKi);
bedKd = scalePID_d(DEFAULT_bedKd);
#endif
#if ENABLED(FWRETRACT)
autoretract_enabled = false;
retract_length = RETRACT_LENGTH;
#if EXTRUDERS > 1
retract_length_swap = RETRACT_LENGTH_SWAP;
#endif
retract_feedrate = RETRACT_FEEDRATE;
retract_zlift = RETRACT_ZLIFT;
retract_recover_length = RETRACT_RECOVER_LENGTH;
#if EXTRUDERS > 1
retract_recover_length_swap = RETRACT_RECOVER_LENGTH_SWAP;
#endif
retract_recover_feedrate = RETRACT_RECOVER_FEEDRATE;
#endif
volumetric_enabled = false;
for (uint8_t q = 0; q < COUNT(filament_size); q++)
filament_size[q] = DEFAULT_NOMINAL_FILAMENT_DIA;
calculate_volumetric_multipliers();
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
void Config_RetrieveSettings() {
int i = EEPROM_OFFSET;
char stored_ver[4];
char ver[4] = EEPROM_VERSION;
EEPROM_READ_VAR(i, stored_ver); //read stored version
// SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]");
if (strncmp(ver, stored_ver, 3) != 0) {
Config_ResetDefault();
}
else {
float dummy = 0;
// version number match
EEPROM_READ_VAR(i, axis_steps_per_unit);
EEPROM_READ_VAR(i, max_feedrate);
EEPROM_READ_VAR(i, max_acceleration_units_per_sq_second);
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
EEPROM_READ_VAR(i, acceleration);
EEPROM_READ_VAR(i, retract_acceleration);
EEPROM_READ_VAR(i, travel_acceleration);
EEPROM_READ_VAR(i, minimumfeedrate);
EEPROM_READ_VAR(i, mintravelfeedrate);
EEPROM_READ_VAR(i, minsegmenttime);
EEPROM_READ_VAR(i, max_xy_jerk);
EEPROM_READ_VAR(i, max_z_jerk);
EEPROM_READ_VAR(i, max_e_jerk);
EEPROM_READ_VAR(i, home_offset);
uint8_t dummy_uint8 = 0, mesh_num_x = 0, mesh_num_y = 0;
EEPROM_READ_VAR(i, dummy_uint8);
EEPROM_READ_VAR(i, mesh_num_x);
EEPROM_READ_VAR(i, mesh_num_y);
#if ENABLED(MESH_BED_LEVELING)
mbl.active = dummy_uint8;
if (mesh_num_x == MESH_NUM_X_POINTS && mesh_num_y == MESH_NUM_Y_POINTS) {
EEPROM_READ_VAR(i, mbl.z_values);
} else {
mbl.reset();
for (int q = 0; q < mesh_num_x * mesh_num_y; q++) EEPROM_READ_VAR(i, dummy);
}
#else
for (int q = 0; q < mesh_num_x * mesh_num_y; q++) EEPROM_READ_VAR(i, dummy);
#endif // MESH_BED_LEVELING
#if DISABLED(AUTO_BED_LEVELING_FEATURE)
float zprobe_zoffset = 0;
#endif
EEPROM_READ_VAR(i, zprobe_zoffset);
#if ENABLED(DELTA)
EEPROM_READ_VAR(i, endstop_adj); // 3 floats
EEPROM_READ_VAR(i, delta_radius); // 1 float
EEPROM_READ_VAR(i, delta_diagonal_rod); // 1 float
EEPROM_READ_VAR(i, delta_segments_per_second); // 1 float
#elif ENABLED(Z_DUAL_ENDSTOPS)
EEPROM_READ_VAR(i, z_endstop_adj);
dummy = 0.0f;
for (int q=5; q--;) EEPROM_READ_VAR(i, dummy);
#else
dummy = 0.0f;
for (int q=6; q--;) EEPROM_READ_VAR(i, dummy);
#endif
#if DISABLED(ULTIPANEL)
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed,
absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
#endif
EEPROM_READ_VAR(i, plaPreheatHotendTemp);
EEPROM_READ_VAR(i, plaPreheatHPBTemp);
EEPROM_READ_VAR(i, plaPreheatFanSpeed);
EEPROM_READ_VAR(i, absPreheatHotendTemp);
EEPROM_READ_VAR(i, absPreheatHPBTemp);
EEPROM_READ_VAR(i, absPreheatFanSpeed);
#if ENABLED(PIDTEMP)
for (int e = 0; e < 4; e++) { // 4 = max extruders currently supported by Marlin
EEPROM_READ_VAR(i, dummy); // Kp
if (e < EXTRUDERS && dummy != DUMMY_PID_VALUE) {
// do not need to scale PID values as the values in EEPROM are already scaled
PID_PARAM(Kp, e) = dummy;
EEPROM_READ_VAR(i, PID_PARAM(Ki, e));
EEPROM_READ_VAR(i, PID_PARAM(Kd, e));
#if ENABLED(PID_ADD_EXTRUSION_RATE)
EEPROM_READ_VAR(i, PID_PARAM(Kc, e));
#else
EEPROM_READ_VAR(i, dummy);
#endif
}
else {
for (int q=3; q--;) EEPROM_READ_VAR(i, dummy); // Ki, Kd, Kc
}
}
#else // !PIDTEMP
// 4 x 4 = 16 slots for PID parameters
for (int q=16; q--;) EEPROM_READ_VAR(i, dummy); // 4x Kp, Ki, Kd, Kc
#endif // !PIDTEMP
#if DISABLED(PID_ADD_EXTRUSION_RATE)
int lpq_len;
#endif
EEPROM_READ_VAR(i, lpq_len);
#if DISABLED(PIDTEMPBED)
float bedKp, bedKi, bedKd;
#endif
EEPROM_READ_VAR(i, dummy); // bedKp
if (dummy != DUMMY_PID_VALUE) {
bedKp = dummy; UNUSED(bedKp);
EEPROM_READ_VAR(i, bedKi);
EEPROM_READ_VAR(i, bedKd);
}
else {
for (int q=2; q--;) EEPROM_READ_VAR(i, dummy); // bedKi, bedKd
}
#if DISABLED(HAS_LCD_CONTRAST)
int lcd_contrast;
#endif
EEPROM_READ_VAR(i, lcd_contrast);
#if ENABLED(SCARA)
EEPROM_READ_VAR(i, axis_scaling); // 3 floats
#else
EEPROM_READ_VAR(i, dummy);
#endif
#if ENABLED(FWRETRACT)
EEPROM_READ_VAR(i, autoretract_enabled);
EEPROM_READ_VAR(i, retract_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_feedrate);
EEPROM_READ_VAR(i, retract_zlift);
EEPROM_READ_VAR(i, retract_recover_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_recover_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_recover_feedrate);
#endif // FWRETRACT
EEPROM_READ_VAR(i, volumetric_enabled);
for (int q = 0; q < 4; q++) {
EEPROM_READ_VAR(i, dummy);
if (q < EXTRUDERS) filament_size[q] = dummy;
}
calculate_volumetric_multipliers();
// Call updatePID (similar to when we have processed M301)
updatePID();
// Report settings retrieved and length
SERIAL_ECHO_START;
SERIAL_ECHO(ver);
SERIAL_ECHOPAIR(" stored settings retrieved (", (unsigned long)i);
SERIAL_ECHOLNPGM(" bytes)");
}
#if ENABLED(EEPROM_CHITCHAT)
Config_PrintSettings();
#endif
}
void computeAxisCommands(float dt)
{
float tempAttCompensation;
if (flightMode == ATTITUDE)
{
attCmd[ROLL ] = rxCommand[ROLL ] * eepromConfig.attitudeScaling;
attCmd[PITCH] = rxCommand[PITCH] * eepromConfig.attitudeScaling;
}
if (flightMode >= ATTITUDE)
{
attPID[ROLL] = updatePID( attCmd[ROLL ], sensors.attitude500Hz[ROLL ], dt, holdIntegrators, &eepromConfig.PID[ROLL_ATT_PID ] );
attPID[PITCH] = updatePID( attCmd[PITCH], -sensors.attitude500Hz[PITCH], dt, holdIntegrators, &eepromConfig.PID[PITCH_ATT_PID] );
}
if (flightMode == RATE)
{
rateCmd[ROLL ] = rxCommand[ROLL ] * eepromConfig.rollAndPitchRateScaling;
rateCmd[PITCH] = rxCommand[PITCH] * eepromConfig.rollAndPitchRateScaling;
}
else
{
rateCmd[ROLL ] = attPID[ROLL ];
rateCmd[PITCH] = attPID[PITCH];
}
///////////////////////////////////
if (headingHoldEngaged == true) // Heading Hold is ON
rateCmd[YAW] = updatePID( headingReference, heading.mag, dt, holdIntegrators, &eepromConfig.PID[HEADING_PID] );
else // Heading Hold is OFF
rateCmd[YAW] = rxCommand[YAW] * eepromConfig.yawRateScaling;
///////////////////////////////////
axisPID[ROLL ] = updatePID( rateCmd[ROLL ], sensors.gyro500Hz[ROLL ], dt, holdIntegrators, &eepromConfig.PID[ROLL_RATE_PID ] );
axisPID[PITCH] = updatePID( rateCmd[PITCH], -sensors.gyro500Hz[PITCH], dt, holdIntegrators, &eepromConfig.PID[PITCH_RATE_PID] );
axisPID[YAW ] = updatePID( rateCmd[YAW ], sensors.gyro500Hz[YAW ], dt, holdIntegrators, &eepromConfig.PID[YAW_RATE_PID ] );
///////////////////////////////////
if (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE) // Manual Mode is ON
{
throttleCmd = rxCommand[THROTTLE];
}
else
{
if ((verticalModeState == ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT) || // Altitude Hold is ON
(verticalModeState == ALT_HOLD_AT_REFERENCE_ALTITUDE) ||
(verticalModeState == ALT_DISENGAGED_THROTTLE_INACTIVE))
{
verticalVelocityCmd = updatePID( altitudeHoldReference, hEstimate, dt, holdIntegrators, &eepromConfig.PID[H_PID] );
}
else // Vertical Velocity Hold is ON
{
verticalVelocityCmd = verticalReferenceCommand * eepromConfig.hDotScaling;
}
throttleCmd = throttleReference + updatePID( verticalVelocityCmd, hDotEstimate, dt, holdIntegrators, &eepromConfig.PID[HDOT_PID] );
// Get Roll Angle, Constrain to +/-20 degrees (default)
tempAttCompensation = constrain(sensors.attitude500Hz[ROLL ], eepromConfig.rollAttAltCompensationLimit, -eepromConfig.rollAttAltCompensationLimit);
// Compute Cosine of Roll Angle and Multiply by Att-Alt Gain
tempAttCompensation = eepromConfig.rollAttAltCompensationGain / cosf(tempAttCompensation);
// Apply Roll Att Compensation to Throttle Command
throttleCmd *= tempAttCompensation;
// Get Pitch Angle, Constrain to +/-20 degrees (default)
tempAttCompensation = constrain(sensors.attitude500Hz[PITCH], eepromConfig.pitchAttAltCompensationLimit, -eepromConfig.pitchAttAltCompensationLimit);
// Compute Cosine of Pitch Angle and Multiply by Att-Alt Gain
tempAttCompensation = eepromConfig.pitchAttAltCompensationGain / cosf(tempAttCompensation);
// Apply Pitch Att Compensation to Throttle Command
throttleCmd *= tempAttCompensation;
}
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
for (short i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef DELTA
endstop_adj[0] = endstop_adj[1] = endstop_adj[2] = 0;
delta_radius = DEFAULT_DELTA_RADIUS;
delta_diagonal_rod = DEFAULT_DELTA_DIAGONAL_ROD;
tower_adj[0] = tower_adj[1] = tower_adj[2] = tower_adj[3] = tower_adj[4] = tower_adj[5] = 0;
max_pos[2] = MANUAL_Z_HOME_POS;
delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND;
set_default_z_probe_offset();
set_delta_constants();
#endif
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif
#ifdef ENABLE_AUTO_BED_LEVELING
zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
#endif
#ifdef DOGLCD
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
void ControlAlgorithm::update()
{
//if frame height and width have changed (changed capture mode perhaps?) reinitialize grid params
int framewidth=kiteColorTracker->FRAME_WIDTH;
if(framewidth!=FRAME_WIDTH){initGrid();}
//get kite position
QVector2D tempVec = kiteColorTracker->kite->getKitePos();
kitePosition = &tempVec;
//get kite heading
QVector2D head = kiteColorTracker->kite->getHeading();
float headingX = head.x();
float headingY = head.y();
float absHead = sqrt(headingX*headingX+headingY*headingY);
QVector2D kiteHeading(headingX/absHead,headingY/absHead); //heading unit vector
qlineKiteHeading.setP1(QPointF(kitePosition->x(),kitePosition->y()));
qlineKiteHeading.setP2(QPointF(kitePosition->x()+kiteHeading.x(),kitePosition->y()+kiteHeading.y()));
//we know first point of kiteAimpoint will be the kite itself
qlineAimPoint.setP1(QPointF(kitePosition->x(),kitePosition->y()));
QVector2D kitePosMem = kiteColorTracker->kite->getPosMem();
QVector2D kiteAimPoint;
float angleError;
bool turnRight;
// generate new path based on quadrant
if(kitePosition->x()>Q1->getLeftX()&&kitePosition->y()<Q1->getBottomY()){
//kite is in first quadrant
//set next aimpoint to quadrant 3
kiteAimPoint=*AIMPOINT_QUAD_3;
//set P2 qline from kite to aimpoint
qlineAimPoint.setP2(QPointF(kiteAimPoint.x(),kiteAimPoint.y()));
//calc error angle using qline angles for full 360 deg. range
angleError= qlineKiteHeading.angleTo(qlineAimPoint);
if(angleError>180){
angleError-=360;
turnRight=false;
}
cv::Mat *framePtr = kiteColorTracker->getFrameHandle();
//draw line from kite to AimPoint
if(framePtr!=NULL){
cv::line(*framePtr,cv::Point(qlineAimPoint.x1(),qlineAimPoint.y1()),cv::Point(qlineAimPoint.x2(),qlineAimPoint.y2()),cv::Scalar(0,255,0),2,2);
cv::putText(*framePtr,floatToStdString(angleError)+" Degrees",cv::Point(kitePosition->x(),kitePosition->y()),2,1,cv::Scalar(0,255,255),2);
}
}
else if(kitePosition->x()<Q2->getRightX()&&kitePosition->y()<Q2->getBottomY()){
//kite is in second quadrant
//ONLY RIGHT TURN PERMITTED
kiteAimPoint=*AIMPOINT_QUAD_4;
//set P2 qline from kite to aimpoint
qlineAimPoint.setP2(QPointF(kiteAimPoint.x(),kiteAimPoint.y()));
//calc error angle using qline angles for full 360 deg. range
angleError= qlineKiteHeading.angleTo(qlineAimPoint);
if(angleError>180){
angleError-=360;
turnRight=false;
}
cv::Mat *framePtr = kiteColorTracker->getFrameHandle();
//draw line from kite to AimPoint
if(framePtr!=NULL){
cv::line(*framePtr,cv::Point(qlineAimPoint.x1(),qlineAimPoint.y1()),cv::Point(qlineAimPoint.x2(),qlineAimPoint.y2()),cv::Scalar(0,255,0),2,2);
cv::putText(*framePtr,floatToStdString(angleError)+" Degrees",cv::Point(kitePosition->x(),kitePosition->y()),2,1,cv::Scalar(0,255,255),2);
}
// run pid on error value OR send error to arduino to compute pid
// IF compute pid here THEN output turn signal to arduino
}
else if(kitePosition->x()<Q3->getRightX()&&kitePosition->y()>Q3->getTopY()){
//kite is in third quadrant
//ONLY RIGHT TURN PERMITTED
kiteAimPoint=*AIMPOINT_QUAD_2;
//set P2 qline from kite to aimpoint
qlineAimPoint.setP2(QPointF(kiteAimPoint.x(),kiteAimPoint.y()));
//calc error angle using qline angles for full 360 deg. range
angleError= qlineKiteHeading.angleTo(qlineAimPoint);
if(angleError>180){
angleError-=360;
turnRight=false;
}
cv::Mat *framePtr = kiteColorTracker->getFrameHandle();
//draw line from kite to AimPoint
if(framePtr!=NULL){
cv::line(*framePtr,cv::Point(qlineAimPoint.x1(),qlineAimPoint.y1()),cv::Point(qlineAimPoint.x2(),qlineAimPoint.y2()),cv::Scalar(0,255,0),2,2);
cv::putText(*framePtr,floatToStdString(angleError)+" Degrees",cv::Point(kitePosition->x(),kitePosition->y()),2,1,cv::Scalar(0,255,255),2);
}
// run pid on error value OR send error to arduino to compute pid
// IF compute pid here THEN output turn signal to arduino
}
else if(kitePosition->x()>Q4->getLeftX()&&kitePosition->y()>Q4->getTopY()){
// determine error
//ONLY LEFT TURN PERMITTED
kiteAimPoint=*AIMPOINT_QUAD_1;
//set P2 qline from kite to aimpoint
qlineAimPoint.setP2(QPointF(kiteAimPoint.x(),kiteAimPoint.y()));
//calc error angle using qline angles for full 360 deg. range
angleError=qlineAimPoint.angle()-qlineKiteHeading.angle();
angleError= qlineKiteHeading.angleTo(qlineAimPoint);
if(angleError>180){
angleError-=360;
}
cv::Mat *framePtr = kiteColorTracker->getFrameHandle();
//draw line from kite to AimPoint
if(framePtr!=NULL){
cv::line(*framePtr,cv::Point(qlineAimPoint.x1(),qlineAimPoint.y1()),cv::Point(qlineAimPoint.x2(),qlineAimPoint.y2()),cv::Scalar(0,255,0),2,2);
cv::putText(*framePtr,floatToStdString(angleError)+" Degrees",cv::Point(kitePosition->x(),kitePosition->y()),2,1,cv::Scalar(0,255,255),2);
}
// run pid on error value OR send error to arduino to compute pid
// IF compute pid here THEN output turn signal to arduino
}
if(kitePosition->x()>Q5->getLeftX()&&kitePosition->y()<Q5->getBottomY()&&kitePosition->x()<Q5->getRightX()&&kitePosition->y()>Q5->getTopY()){
//kite is in POWER ZONE
cv::Mat *framePtr = kiteColorTracker->getFrameHandle();
//draw line from kite to AimPoint
if(framePtr!=NULL){
cv::putText(*framePtr,"POWER",cv::Point(kiteColorTracker->FRAME_WIDTH/2,Q5->getTopY()),2,2,cv::Scalar(0,0,255),2);
}
// run pid on error value OR send error to arduino to compute pid
// IF compute pid here THEN output turn signal to arduino
}
// update PID process variable (ie. input value)
//pid->setProcessValue(abs(angleError)); // angle error must be both posative and negative to know what side we are on
pid->setProcessValue(angleError); // Adjusted the input limits to go from -45.0 deg to +45.0 deg
// compute new pid output
updatePID();
if(angleError<0){pidOutput=-pidOutput;}
//qDebug()<<"PID output: "<<pidOutput;
drawToFrame(kitePosMem,kiteHeading);
//save position data for next interation
kiteColorTracker->kite->setPosMem(kitePosition->x(),kitePosition->y());
//emit data for control options window display
emit(dataUpdated());
//finally, emit signal for turn command to kite
//if autopilot is engaged
if(autoPilotOn)
emit(writeToArduino("t"+QString::number(pidOutput)+"/"));
}
void Config_RetrieveSettings()
{
int i=EEPROM_OFFSET;
char stored_ver[4];
char ver[4]=EEPROM_VERSION;
EEPROM_READ_VAR(i,stored_ver); //read stored version
// SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]");
if (strncmp(ver,stored_ver,3) == 0)
{
// version number match
EEPROM_READ_VAR(i,axis_steps_per_unit);
EEPROM_READ_VAR(i,max_feedrate);
EEPROM_READ_VAR(i,max_acceleration_units_per_sq_second);
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
EEPROM_READ_VAR(i,acceleration);
EEPROM_READ_VAR(i,retract_acceleration);
EEPROM_READ_VAR(i,minimumfeedrate);
EEPROM_READ_VAR(i,mintravelfeedrate);
EEPROM_READ_VAR(i,minsegmenttime);
EEPROM_READ_VAR(i,max_xy_jerk);
EEPROM_READ_VAR(i,max_z_jerk);
EEPROM_READ_VAR(i,max_e_jerk);
EEPROM_READ_VAR(i,add_homeing);
#ifndef ULTIPANEL
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed;
int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
#endif
EEPROM_READ_VAR(i,plaPreheatHotendTemp);
EEPROM_READ_VAR(i,plaPreheatHPBTemp);
EEPROM_READ_VAR(i,plaPreheatFanSpeed);
EEPROM_READ_VAR(i,absPreheatHotendTemp);
EEPROM_READ_VAR(i,absPreheatHPBTemp);
EEPROM_READ_VAR(i,absPreheatFanSpeed);
#ifndef PIDTEMP
float Kp,Ki,Kd;
#endif
// do not need to scale PID values as the values in EEPROM are already scaled
EEPROM_READ_VAR(i,Kp);
EEPROM_READ_VAR(i,Ki);
EEPROM_READ_VAR(i,Kd);
EEPROM_READ_VAR(i,motor_current_setting);
#ifdef ENABLE_ULTILCD2
EEPROM_READ_VAR(i,led_brightness_level);
EEPROM_READ_VAR(i,led_mode);
#else
uint8_t dummyByte;
EEPROM_READ_VAR(i,dummyByte);
EEPROM_READ_VAR(i,dummyByte);
#endif
EEPROM_READ_VAR(i,retract_length);
EEPROM_READ_VAR(i,retract_feedrate);
// Call updatePID (similar to when we have processed M301)
updatePID();
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Stored settings retrieved");
}
else
{
Config_ResetDefault();
}
if (strncmp_P(ver, PSTR("V010"), 3) == 0)
{
i = EEPROM_OFFSET + 84;
EEPROM_READ_VAR(i,add_homeing);
}
Config_PrintSettings();
//ignore EEPROM extruder steps/mm and current
float temp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
axis_steps_per_unit[3]=temp1[3]; //overide EEPROM steps
float temp2[]=DEFAULT_PWM_MOTOR_CURRENT;
motor_current_setting[2] = temp2[2]; // overide EEPROM current
}
///////////////////////////////////////////////////////////////////////////////
// Compute Axis Commands
///////////////////////////////////////////////////////////////////////////////
void computeAxisCommands(float dt)
{
float error;
float tempAttCompensation;
///////////////////////////////////
// Attitude mode is ON, apply attitude stabilization to input commands BEFORE rate calculations
if (flightMode >= ATTITUDE)
{
if (flightMode == ATTITUDE)
{
// Scale user input in attitude mode
attCmd[ROLL ] = rxCommand[ROLL ] * eepromConfig.attitudeScaling;
attCmd[PITCH] = rxCommand[PITCH] * eepromConfig.attitudeScaling;
}
// Compute error terms for roll and pitch in attitude mode (User input compared to vehicle attitude)
// Apply error terms to PID controllers as feedback
error = standardRadianFormat(attCmd[ROLL] - sensors.attitude500Hz[ROLL]);
attPID[ROLL] = updatePID(error, dt, pidReset, &eepromConfig.PID[ROLL_ATT_PID]);
error = standardRadianFormat(attCmd[PITCH] + sensors.attitude500Hz[PITCH]);
attPID[PITCH] = updatePID(error, dt, pidReset, &eepromConfig.PID[PITCH_ATT_PID]);
}
// if (flightMode == GPS)//TODO
// {
//
// }
// Rate mode is ON
if (flightMode == RATE)
{
// Scale user input in rate mode
rateCmd[ROLL ] = rxCommand[ROLL ] * eepromConfig.rollAndPitchRateScaling;
rateCmd[PITCH] = rxCommand[PITCH] * eepromConfig.rollAndPitchRateScaling;
}
// Attitude mode is ON
else
{
// Take control input from output of attitude PIDs
rateCmd[ROLL ] = attPID[ROLL ];
rateCmd[PITCH] = attPID[PITCH];
}
///////////////////////////////////
// Heading Hold is ON, compute error term for yaw, apply to PID, and use PID calculation for output
if (headingHoldEngaged == true)
{
error = standardRadianFormat(headingReference - heading.mag);
rateCmd[YAW] = updatePID(error, dt, pidReset, &eepromConfig.PID[HEADING_PID]);
}
// Heading Hold is OFF, get yaw direct from user input
else
rateCmd[YAW] = rxCommand[YAW] * eepromConfig.yawRateScaling;
///////////////////////////////////
// Compute error terms for roll, pitch and yaw from rate OR attitude inputs found above
// Apply error terms to rate PID controllers as feedback based on gyro data
// Rate PIDs based off error on gyro only
error = rateCmd[ROLL] - sensors.gyro500Hz[ROLL];
ratePID[ROLL] = updatePID(error, dt, pidReset, &eepromConfig.PID[ROLL_RATE_PID ]);
error = rateCmd[PITCH] + sensors.gyro500Hz[PITCH];
ratePID[PITCH] = updatePID(error, dt, pidReset, &eepromConfig.PID[PITCH_RATE_PID]);
error = rateCmd[YAW] - sensors.gyro500Hz[YAW];
ratePID[YAW] = updatePID(error, dt, pidReset, &eepromConfig.PID[YAW_RATE_PID ]);
///////////////////////////////////
// Manual Mode is ON, no altitude hold
if (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE)
throttleCmd = rxCommand[THROTTLE];
else
{
// Altitude Hold is ON
if ((verticalModeState == ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT) || (verticalModeState == ALT_HOLD_AT_REFERENCE_ALTITUDE) || (verticalModeState == ALT_DISENGAGED_THROTTLE_INACTIVE))
{
// Compute error term for altitude position based off height estimation from sensors
// Use as feedback to vertical position hold PID, which serves as input to vertical velocity PID
// altitudeHoldReference depends on particular type of altitude hold enabled (fixed, at engagement, or throttle inactive)
error = altitudeHoldReference - hEstimate;
verticalVelocityCmd = updatePID(error, dt, pidReset, &eepromConfig.PID[H_PID]);
}
// Vertical Velocity Hold is ON
else
{
// Get vertical velocity directly from velocity reference command (user input)
verticalVelocityCmd = verticalReferenceCommand * eepromConfig.hDotScaling;
}
// Compute error term for vertical velocity hold PID based of estimated vertical velocity (hDot)
// Use as feedback to vertical velocity hold PID
error = verticalVelocityCmd - hDotEstimate;
throttleCmd = throttleReference + updatePID(error, dt, pidReset, &eepromConfig.PID[HDOT_PID]);
// Get Roll Angle, Constrain to +/-20 degrees (default)
tempAttCompensation = constrain(sensors.attitude500Hz[ROLL ], eepromConfig.rollAttAltCompensationLimit, -eepromConfig.rollAttAltCompensationLimit);
// Compute Cosine of Roll Angle and Multiply by Att-Alt Gain
tempAttCompensation = eepromConfig.rollAttAltCompensationGain / cosf(tempAttCompensation);
// Apply Roll Att Compensation to Throttle Command
throttleCmd *= tempAttCompensation;
// Get Pitch Angle, Constrain to +/-20 degrees (default)
tempAttCompensation = constrain(sensors.attitude500Hz[PITCH], eepromConfig.pitchAttAltCompensationLimit, -eepromConfig.pitchAttAltCompensationLimit);
// Compute Cosine of Pitch Angle and Multiply by Att-Alt Gain
tempAttCompensation = eepromConfig.pitchAttAltCompensationGain / cosf(tempAttCompensation);
// Apply Pitch Att Compensation to Throttle Command
throttleCmd *= tempAttCompensation;
}
///////////////////////////////////
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
files (startup_stm32f40xx.s/startup_stm32f427x.s) before to branch to
application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f4xx.c file
*/
/* USART configuration -----------------------------------------------------*/
USART_Config();
/* SysTick configuration ---------------------------------------------------*/
SysTickConfig();
/* LEDs configuration ------------------------------------------------------*/
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDOn(LED3);//orange
STM_EVAL_LEDOn(LED4);//verte
STM_EVAL_LEDOn(LED5);//rouge
STM_EVAL_LEDOn(LED6);//bleue
//PWM config (motor control)
TIM1_Config();
PWM1_Config(10000);
/* Tamper Button Configuration ---------------------------------------------*/
STM_EVAL_PBInit(BUTTON_USER,BUTTON_MODE_GPIO);
//Set motor speed
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
/* Initialization of the accelerometer -------------------------------------*/
MPU6050_I2C_Init();
MPU6050_Initialize();
if (MPU6050_TestConnection()) {
// connection success
STM_EVAL_LEDOff(LED3);
}else{
STM_EVAL_LEDOff(LED4);
}
//Calibration process
// Use the following global variables and access functions
// to calibrate the acceleration sensor
calibrate_sensors();
zeroError();
//Ready to receive message
/* Enable DMA USART RX Stream */
DMA_Cmd(USARTx_RX_DMA_STREAM,ENABLE);
/* Enable USART DMA RX Requsts */
USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE);
while(1){
//--------------------------------------------------------
//------ Used to configure the speed controller ----------
//--------------------------------------------------------
// press blue button to force motor at SPEED_100
if (STM_EVAL_PBGetState(BUTTON_USER)){
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
Delay(100);
}
//--------------------------------------------------------
//------ Get gyro information ----------
//--------------------------------------------------------
// Read the raw values.
MPU6050_GetRawAccelGyro(AccelGyro);
// Get the time of reading for rotation computations
unsigned long t_now = millis();
STM_EVAL_LEDToggle(LED5);
// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet:
// 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 – (340 * 35) = -12412
//dT = ( (double) AccelGyro[TEMP] + 12412.0) / 340.0;
// Convert gyro values to degrees/sec
gyro_x = (AccelGyro[GYRO_X] - base_x_gyro) / FSSEL;
gyro_y = (AccelGyro[GYRO_Y] - base_y_gyro) / FSSEL;
gyro_z = (AccelGyro[GYRO_Z] - base_z_gyro) / FSSEL;
// Get raw acceleration values
accel_x = AccelGyro[ACC_X];
accel_y = AccelGyro[ACC_Y];
accel_z = AccelGyro[ACC_Z];
// Get angle values from accelerometer
//float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS2DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS2DEGREES;
//float accel_angle_z = 0;
//// Compute the (filtered) gyro angles
//Get the value in second, a tick is every 10ms
dt = (t_now - last_read_time)/100.0;
float gyro_angle_x = gyro_x*dt + lastAngle[X];//get_last_x_angle();
float gyro_angle_y = gyro_y*dt + lastAngle[Y];//(get_last_y_angle();
float gyro_angle_z = gyro_z*dt + lastAngle[Z];//get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + lastGyroAngle[X];//get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + lastGyroAngle[Y];//get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + lastGyroAngle[Z];//get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle – choice of alpha is
// estimated now. Alpha depends on the sampling rate…
float alpha = 0.96;
angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x;
angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y;
angle_z = gyro_angle_z; //Accelerometer doesn’t give z-angle
//printf("%4.2f %4.2f %4.2f\r\n",angle_x,angle_y,angle_z);
//// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
//Stabilisation
// Stable Mode
angl = getAngleFromRC(rcBluetooth[ROLL]);
levelRoll = (getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * PID[LEVELROLL].P;
levelPitch = (getAngleFromRC(rcBluetooth[PITCH]) - angle_y) * PID[LEVELPITCH].P;
// Check if pilot commands are not in hover, don't auto trim
// if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) {
// zeroIntegralError();
// }
// else {
PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((getAngleFromRC(rcBluetooth[PITCH]) + angle_y) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
// }
//motors.setMotorAxisCommand(ROLL,
motor[ROLL] = updatePID(rcBluetooth[ROLL] + levelRoll, gyro_x + 1500, &PID[LEVELGYROROLL],dt) + PID[LEVELROLL].integratedError;//);
//motors.setMotorAxisCommand(PITCH,
motor[PITCH] = updatePID(rcBluetooth[PITCH] + levelPitch, gyro_y + 1500, &PID[LEVELGYROPITCH],dt) + PID[LEVELPITCH].integratedError;//);
getLastSpeedFromMsg();
PWM_SetDC(1, SPEED_0 + SPEED_RANGE*rcSpeed[1] + motor[ROLL] *0.10f); //PE9 | PC6//ON 2ms
//Send data on UART
*(float*)(aTxBuffer) = angle_x;
*(float*)(aTxBuffer+4) = angle_y;
*(float*)(aTxBuffer+8) = angle_z;
*(float*)(aTxBuffer+12) = motor[ROLL];
*(float*)(aTxBuffer+16) = motor[PITCH];
sendTxDMA((uint32_t)aTxBuffer,20);
//Wait a little bit
Delay(3); //30 ms
}
}
void Config_ResetDefault() {
float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[] = DEFAULT_MAX_FEEDRATE;
long tmp3[] = DEFAULT_MAX_ACCELERATION;
for (int i = 0; i < NUM_AXIS; i++) {
axis_steps_per_unit[i] = tmp1[i];
max_feedrate[i] = tmp2[i];
max_acceleration_units_per_sq_second[i] = tmp3[i];
#ifdef SCARA
if (i < sizeof(axis_scaling) / sizeof(*axis_scaling))
axis_scaling[i] = 1;
#endif
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration = DEFAULT_ACCELERATION;
retract_acceleration = DEFAULT_RETRACT_ACCELERATION;
travel_acceleration = DEFAULT_TRAVEL_ACCELERATION;
minimumfeedrate = DEFAULT_MINIMUMFEEDRATE;
minsegmenttime = DEFAULT_MINSEGMENTTIME;
mintravelfeedrate = DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk = DEFAULT_XYJERK;
max_z_jerk = DEFAULT_ZJERK;
max_e_jerk = DEFAULT_EJERK;
add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0;
#ifdef DELTA
endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
recalc_delta_settings(delta_radius, delta_diagonal_rod);
#endif
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif
#ifdef LEVEL_SENSOR
zprobe_zoffset = eeprom::StorageManager::single::instance().getOffset();
#endif
#ifdef DOGLCD
lcd_contrast = DEFAULT_LCD_CONTRAST;
#endif
#ifdef PIDTEMP
#ifdef PID_PARAMS_PER_EXTRUDER
for (int e = 0; e < EXTRUDERS; e++)
#else
int e = 0; // only need to write once
#endif
{
PID_PARAM(Kp, e) = DEFAULT_Kp;
PID_PARAM(Ki, e) = scalePID_i(DEFAULT_Ki);
PID_PARAM(Kd, e) = scalePID_d(DEFAULT_Kd);
#ifdef PID_ADD_EXTRUSION_RATE
PID_PARAM(Kc, e) = DEFAULT_Kc;
#endif
}
// call updatePID (similar to when we have processed M301)
updatePID();
#endif // PIDTEMP
#ifdef FWRETRACT
autoretract_enabled = false;
retract_length = RETRACT_LENGTH;
#if EXTRUDERS > 1
retract_length_swap = RETRACT_LENGTH_SWAP;
#endif
retract_feedrate = RETRACT_FEEDRATE;
retract_zlift = RETRACT_ZLIFT;
retract_recover_length = RETRACT_RECOVER_LENGTH;
#if EXTRUDERS > 1
retract_recover_length_swap = RETRACT_RECOVER_LENGTH_SWAP;
#endif
retract_recover_feedrate = RETRACT_RECOVER_FEEDRATE;
#endif
volumetric_enabled = false;
filament_size[0] = DEFAULT_NOMINAL_FILAMENT_DIA;
#if EXTRUDERS > 1
filament_size[1] = DEFAULT_NOMINAL_FILAMENT_DIA;
#if EXTRUDERS > 2
filament_size[2] = DEFAULT_NOMINAL_FILAMENT_DIA;
#if EXTRUDERS > 3
filament_size[3] = DEFAULT_NOMINAL_FILAMENT_DIA;
#endif
#endif
#endif
calculate_volumetric_multipliers();
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
void Config_StoreSettings() {
float dummy = 0.0f;
char ver[4] = "000";
int i = EEPROM_OFFSET;
EEPROM_WRITE_VAR(i, ver); // invalidate data first
EEPROM_WRITE_VAR(i, axis_steps_per_unit);
EEPROM_WRITE_VAR(i, max_feedrate);
EEPROM_WRITE_VAR(i, max_acceleration_units_per_sq_second);
EEPROM_WRITE_VAR(i, acceleration);
EEPROM_WRITE_VAR(i, retract_acceleration);
EEPROM_WRITE_VAR(i, minimumfeedrate);
EEPROM_WRITE_VAR(i, mintravelfeedrate);
EEPROM_WRITE_VAR(i, minsegmenttime);
EEPROM_WRITE_VAR(i, max_xy_jerk);
EEPROM_WRITE_VAR(i, max_z_jerk);
EEPROM_WRITE_VAR(i, max_e_jerk);
EEPROM_WRITE_VAR(i, add_homing);
#ifdef DELTA
EEPROM_WRITE_VAR(i, endstop_adj); // 3 floats
EEPROM_WRITE_VAR(i, delta_radius); // 1 float
EEPROM_WRITE_VAR(i, delta_diagonal_rod); // 1 float
EEPROM_WRITE_VAR(i, delta_segments_per_second); // 1 float
#else
dummy = 0.0f;
for (int q=6; q--;) EEPROM_WRITE_VAR(i, dummy);
#endif
#ifndef ULTIPANEL
int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED,
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
#endif // !ULTIPANEL
EEPROM_WRITE_VAR(i, plaPreheatHotendTemp);
EEPROM_WRITE_VAR(i, plaPreheatHPBTemp);
EEPROM_WRITE_VAR(i, plaPreheatFanSpeed);
EEPROM_WRITE_VAR(i, absPreheatHotendTemp);
EEPROM_WRITE_VAR(i, absPreheatHPBTemp);
EEPROM_WRITE_VAR(i, absPreheatFanSpeed);
EEPROM_WRITE_VAR(i, zprobe_zoffset);
for (int e = 0; e < 4; e++) {
#ifdef PIDTEMP
if (e < EXTRUDERS) {
EEPROM_WRITE_VAR(i, PID_PARAM(Kp, e));
EEPROM_WRITE_VAR(i, PID_PARAM(Ki, e));
EEPROM_WRITE_VAR(i, PID_PARAM(Kd, e));
#ifdef PID_ADD_EXTRUSION_RATE
EEPROM_WRITE_VAR(i, PID_PARAM(Kc, e));
#else
dummy = 1.0f; // 1.0 = default kc
EEPROM_WRITE_VAR(i, dummy);
#endif
}
else {
#else // !PIDTEMP
{
#endif // !PIDTEMP
dummy = DUMMY_PID_VALUE;
EEPROM_WRITE_VAR(i, dummy);
dummy = 0.0f;
for (int q = 3; q--;) EEPROM_WRITE_VAR(i, dummy);
}
} // Extruders Loop
#ifndef DOGLCD
int lcd_contrast = 32;
#endif
EEPROM_WRITE_VAR(i, lcd_contrast);
#ifdef SCARA
EEPROM_WRITE_VAR(i, axis_scaling); // 3 floats
#else
dummy = 1.0f;
EEPROM_WRITE_VAR(i, dummy);
#endif
#ifdef FWRETRACT
EEPROM_WRITE_VAR(i, autoretract_enabled);
EEPROM_WRITE_VAR(i, retract_length);
#if EXTRUDERS > 1
EEPROM_WRITE_VAR(i, retract_length_swap);
#else
dummy = 0.0f;
EEPROM_WRITE_VAR(i, dummy);
#endif
EEPROM_WRITE_VAR(i, retract_feedrate);
EEPROM_WRITE_VAR(i, retract_zlift);
EEPROM_WRITE_VAR(i, retract_recover_length);
#if EXTRUDERS > 1
EEPROM_WRITE_VAR(i, retract_recover_length_swap);
#else
dummy = 0.0f;
EEPROM_WRITE_VAR(i, dummy);
#endif
EEPROM_WRITE_VAR(i, retract_recover_feedrate);
#endif // FWRETRACT
EEPROM_WRITE_VAR(i, volumetric_enabled);
// Save filament sizes
for (int q = 0; q < 4; q++) {
if (q < EXTRUDERS) dummy = filament_size[q];
EEPROM_WRITE_VAR(i, dummy);
}
int storageSize = i;
char ver2[4] = EEPROM_VERSION;
int j = EEPROM_OFFSET;
EEPROM_WRITE_VAR(j, ver2); // validate data
// Report storage size
SERIAL_ECHO_START;
SERIAL_ECHOPAIR("Settings Stored (", (unsigned long)i);
SERIAL_ECHOLNPGM(" bytes)");
}
void Config_RetrieveSettings() {
int i = EEPROM_OFFSET;
char stored_ver[4];
char ver[4] = EEPROM_VERSION;
EEPROM_READ_VAR(i, stored_ver); //read stored version
// SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]");
if (strncmp(ver, stored_ver, 3) != 0) {
Config_ResetDefault();
}
else {
float dummy = 0;
// version number match
EEPROM_READ_VAR(i, axis_steps_per_unit);
EEPROM_READ_VAR(i, max_feedrate);
EEPROM_READ_VAR(i, max_acceleration_units_per_sq_second);
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates();
EEPROM_READ_VAR(i, acceleration);
EEPROM_READ_VAR(i, retract_acceleration);
EEPROM_READ_VAR(i, minimumfeedrate);
EEPROM_READ_VAR(i, mintravelfeedrate);
EEPROM_READ_VAR(i, minsegmenttime);
EEPROM_READ_VAR(i, max_xy_jerk);
EEPROM_READ_VAR(i, max_z_jerk);
EEPROM_READ_VAR(i, max_e_jerk);
EEPROM_READ_VAR(i, add_homing);
#ifdef DELTA
EEPROM_READ_VAR(i, endstop_adj); // 3 floats
EEPROM_READ_VAR(i, delta_radius); // 1 float
EEPROM_READ_VAR(i, delta_diagonal_rod); // 1 float
EEPROM_READ_VAR(i, delta_segments_per_second); // 1 float
#else
for (int q=6; q--;) EEPROM_READ_VAR(i, dummy);
#endif
#ifndef ULTIPANEL
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed,
absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
#endif
EEPROM_READ_VAR(i, plaPreheatHotendTemp);
EEPROM_READ_VAR(i, plaPreheatHPBTemp);
EEPROM_READ_VAR(i, plaPreheatFanSpeed);
EEPROM_READ_VAR(i, absPreheatHotendTemp);
EEPROM_READ_VAR(i, absPreheatHPBTemp);
EEPROM_READ_VAR(i, absPreheatFanSpeed);
EEPROM_READ_VAR(i, zprobe_zoffset);
#ifdef PIDTEMP
for (int e = 0; e < 4; e++) { // 4 = max extruders currently supported by Marlin
EEPROM_READ_VAR(i, dummy);
if (e < EXTRUDERS && dummy != DUMMY_PID_VALUE) {
// do not need to scale PID values as the values in EEPROM are already scaled
PID_PARAM(Kp, e) = dummy;
EEPROM_READ_VAR(i, PID_PARAM(Ki, e));
EEPROM_READ_VAR(i, PID_PARAM(Kd, e));
#ifdef PID_ADD_EXTRUSION_RATE
EEPROM_READ_VAR(i, PID_PARAM(Kc, e));
#else
EEPROM_READ_VAR(i, dummy);
#endif
}
else {
for (int q=3; q--;) EEPROM_READ_VAR(i, dummy); // Ki, Kd, Kc
}
}
#else // !PIDTEMP
// 4 x 4 = 16 slots for PID parameters
for (int q=16; q--;) EEPROM_READ_VAR(i, dummy); // 4x Kp, Ki, Kd, Kc
#endif // !PIDTEMP
#ifndef DOGLCD
int lcd_contrast;
#endif
EEPROM_READ_VAR(i, lcd_contrast);
#ifdef SCARA
EEPROM_READ_VAR(i, axis_scaling); // 3 floats
#else
EEPROM_READ_VAR(i, dummy);
#endif
#ifdef FWRETRACT
EEPROM_READ_VAR(i, autoretract_enabled);
EEPROM_READ_VAR(i, retract_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_feedrate);
EEPROM_READ_VAR(i, retract_zlift);
EEPROM_READ_VAR(i, retract_recover_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_recover_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_recover_feedrate);
#endif // FWRETRACT
EEPROM_READ_VAR(i, volumetric_enabled);
for (int q = 0; q < 4; q++) {
EEPROM_READ_VAR(i, dummy);
if (q < EXTRUDERS) filament_size[q] = dummy;
}
calculate_volumetric_multipliers();
// Call updatePID (similar to when we have processed M301)
updatePID();
// Report settings retrieved and length
SERIAL_ECHO_START;
SERIAL_ECHO(ver);
SERIAL_ECHOPAIR(" stored settings retrieved (", (unsigned long)i);
SERIAL_ECHOLNPGM(" bytes)");
}
#ifdef EEPROM_CHITCHAT
Config_PrintSettings();
#endif
}
//////////////////////////////////////////////////////////////////////////////
/////////////////////////// processHeadingHold ///////////////////////////////
//////////////////////////////////////////////////////////////////////////////
void processHeading(void)
{
if (headingHoldConfig == ON) {
#if defined(HeadingMagHold) || defined(AeroQuadMega_CHR6DM) || defined(APM_OP_CHR6DM)
heading = degrees(flightAngle->getHeading(YAW));
#else
heading = degrees(gyro.getHeading());
#endif
// Always center relative heading around absolute heading chosen during yaw command
// This assumes that an incorrect yaw can't be forced on the AeroQuad >180 or <-180 degrees
// This is done so that AeroQuad does not accidentally hit transition between 0 and 360 or -180 and 180
// AKA - THERE IS A BUG HERE - if relative heading is greater than 180 degrees, the PID will swing from negative to positive
// Doubt that will happen as it would have to be uncommanded.
relativeHeading = heading - setHeading;
if (heading <= (setHeading - 180)) relativeHeading += 360;
if (heading >= (setHeading + 180)) relativeHeading -= 360;
// Apply heading hold only when throttle high enough to start flight
if (receiver.getData(THROTTLE) > MINCHECK ) {
if ((receiver.getData(YAW) > (MIDCOMMAND + 25)) || (receiver.getData(YAW) < (MIDCOMMAND - 25))) {
// If commanding yaw, turn off heading hold and store latest heading
setHeading = heading;
headingHold = 0;
PID[HEADING].integratedError = 0;
headingHoldState = OFF;
headingTime = currentTime;
}
else {
if (relativeHeading < .25 && relativeHeading > -.25) {
headingHold = 0;
PID[HEADING].integratedError = 0;
}
else if (headingHoldState == OFF) { // quick fix to soften heading hold on new heading
if ((currentTime - headingTime) > 500000) {
headingHoldState = ON;
headingTime = currentTime;
setHeading = heading;
headingHold = 0;
}
}
else {
// No new yaw input, calculate current heading vs. desired heading heading hold
// Relative heading is always centered around zero
headingHold = updatePID(0, relativeHeading, &PID[HEADING]);
headingTime = currentTime; // quick fix to soften heading hold, wait 100ms before applying heading hold
}
}
}
else {
// minimum throttle not reached, use off settings
setHeading = heading;
headingHold = 0;
PID[HEADING].integratedError = 0;
}
}
// NEW SI Version
commandedYaw = constrain(receiver.getSIData(YAW) + radians(headingHold), -PI, PI);
motors.setMotorAxisCommand(YAW, updatePID(commandedYaw, gyro.getData(YAW), &PID[YAW]));
// uses flightAngle unbias rate
//motors.setMotorAxisCommand(YAW, updatePID(commandedYaw, flightAngle->getGyroUnbias(YAW), &PID[YAW]));
}
asynStatus ReadASCII::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
//Deals with the user changing the SP target
int function = pasynUser->reason;
asynStatus status = asynSuccess;
int rampOn, LUTOn;
const char *paramName;
const char* functionName = "writeFloat64";
/* Set the parameter in the parameter library. */
status = (asynStatus)setDoubleParam(function, value);
/* Fetch the parameter string name for possible use in debugging */
getParamName(function, ¶mName);
if (function == P_Target)
{
//check ramp on
getIntegerParam(P_RampOn, &rampOn);
//check lookup on
getIntegerParam(P_LookUpOn, &LUTOn);
if (rampOn) {
double startTemp= 0.0;
//get current temperature and set as SP
getDoubleParam(P_CurTemp, &startTemp);
setDoubleParam(P_SPOut, startTemp);
//update PIDs
if (LUTOn)
{
updatePID(getSPInd(startTemp));
}
//start ramping
setIntegerParam(P_Ramping, 1);
//send event to start ramp
epicsEventSignal(eventId_);
} else {
//directly output SP
setDoubleParam(P_SPOut, value);
//update PIDs
if (LUTOn)
{
updatePID(getSPInd(value));
}
}
}
/* Do callbacks so higher layers see any changes */
status = (asynStatus)callParamCallbacks();
if (status)
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
"%s:%s: status=%d, function=%d",
driverName, functionName, status, function);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d\n",
driverName, functionName, function);
return status;
}
void Config_ResetDefault()
{
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
float tmp2[]=DEFAULT_MAX_FEEDRATE;
long tmp3[]=DEFAULT_MAX_ACCELERATION;
for (short i=0;i<4;i++)
{
axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[i];
}
// steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates();
acceleration=DEFAULT_ACCELERATION;
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK;
max_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK;
add_homeing[0] = add_homeing[1] = add_homeing[2] = 0;
#ifdef HYSTERESIS_H
menu_hysteresis_X=X_DEFAULT_HYSTERESIS_MM;
menu_hysteresis_Y=Y_DEFAULT_HYSTERESIS_MM;
#endif
#ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP;
absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP;
absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
nylonPreheatHotendTemp = NYLON_PREHEAT_HOTEND_TEMP;
nylonPreheatHPBTemp = NYLON_PREHEAT_HPB_TEMP;
nylonPreheatFanSpeed = NYLON_PREHEAT_FAN_SPEED;
pvaPreheatHotendTemp = PVA_PREHEAT_HOTEND_TEMP;
pvaPreheatHPBTemp = PVA_PREHEAT_HPB_TEMP;
pvaPreheatFanSpeed = PVA_PREHEAT_FAN_SPEED;
int laywoodPreheatHotendTemp = LAYWOOD_PREHEAT_HOTEND_TEMP;
int laywoodPreheatHPBTemp = LAYWOOD_PREHEAT_HPB_TEMP;
int laywoodPreheatFanSpeed = LAYWOOD_PREHEAT_FAN_SPEED;
int laybrickPreheatHotendTemp = LAYBRICK_PREHEAT_HOTEND_TEMP;
int laybrickPreheatHPBTemp = LAYBRICK_PREHEAT_HPB_TEMP;
int laybrickPreheatFanSpeed = LAYBRICK_PREHEAT_FAN_SPEED;
#endif
#ifdef PIDTEMP
Kp = DEFAULT_Kp;
Ki = scalePID_i(DEFAULT_Ki);
Kd = scalePID_d(DEFAULT_Kd);
// call updatePID (similar to when we have processed M301)
updatePID();
#ifdef PID_ADD_EXTRUSION_RATE
Kc = DEFAULT_Kc;
#endif//PID_ADD_EXTRUSION_RATE
#endif//PIDTEMP
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");
}
