自学教程：C++ updatePID函数代码示例

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//PIDTEMPzprobe_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 drivervoid 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 drivervoid 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//PIDTEMPSERIAL_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//.........这里部分代码省略.........

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],//.........这里部分代码省略.........

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//.........这里部分代码省略.........

//.........这里部分代码省略.........    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//PIDTEMPSERIAL_ECHO_START;SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");}

//.........这里部分代码省略.........            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)//.........这里部分代码省略.........

//.........这里部分代码省略.........      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 
C++ updateParameters函数代码示例
C++ updateObjects函数代码示例