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"); }