void ExtruderTask(void) { SysTick_t now = GetSystemTick(); if(bHeating && now - lastUpdatingTime > EXTRUDER_UPDATE_PERIOD) { int output; #if EXTRUDER_THERMO_USING_ADC uint16_t adc_val = Analog_GetChannelValue(Extruder1Therm_ADC_Ch); currentTemp = EXTRUDER_ADC_TO_TEMP(adc_val); // DBG_MSG("ADC Val: %d %d", adc_val, currentTemp); #else currentTemp = MAX6675_Read_Value(); #endif lastUpdatingTime = now; if(currentTemp < 0) { ERR_MSG("Thermcouple not connected!", 0); return; } output = PID_Update(&pid, targetTemp - currentTemp); //转为百分比 output /= 400; Extruder_SetOutput(output); } }
/** * @brief Update all PID Struct in the List * * Handle the Update Divider Here. * @see PIDList * @see PID_Update * */ void PID_UpdateAll(void){ int i; PIDUpdateCount++; for(i = 0; i < PIDListCount; i++){ if(PIDUpdateCount % PIDList[i]->update_divider == 0) PID_Update(PIDList[i]); } // if(PIDUpdateCount == MaxCount)PIDUpdateCount = 0; return; }
float PID_UpdateX(float actual) { return PID_Update(&pid_X, actual); }
void calculate_pid() { // Get orientation data from IMU and depth from depth sensor get_imu_orientation(¤t_orientation); current_orientation.depth = get_average_depth(); /* current_orientation.pitch and .roll are controlled towards zero * and .depth is controlled towards target_orientation.depth */ // update the controller with the new pitch and roll values PID_Update (&PID_Roll, current_orientation.roll); PID_Update (&PID_Pitch, current_orientation.pitch); // normalize yaw difference between -180 and 180 degrees double delta_yaw = (target_orientation.yaw - current_orientation.yaw); delta_yaw = fmod(delta_yaw, MAX_YAW); if (delta_yaw > MAX_YAW/2) { delta_yaw -= MAX_YAW; } else if (delta_yaw < -MAX_YAW/2) { delta_yaw += MAX_YAW; } PID_Update (&PID_Yaw, delta_yaw); // calculate depth difference double delta_depth = (target_orientation.depth - current_orientation.depth); PID_Update (&PID_Depth, delta_depth); // calculate the force required double Roll_Force_Needed = FACTOR_PID_ROLL_TO_FORCE * PID_Output(&PID_Roll); double Pitch_Force_Needed = FACTOR_PID_PITCH_TO_FORCE * PID_Output(&PID_Pitch); double Depth_Force_Needed = FACTOR_PID_DEPTH_TO_FORCE * PID_Output(&PID_Depth); double Yaw_Force_Needed = FACTOR_PID_YAW_TO_FORCE * PID_Output(&PID_Yaw); double Forward_Force_Needed = FACTOR_SPEED_TO_FORCE * target_orientation.speed; /** orientation stability * If the COM is off center we would have some sort of factors here instead of 0.5 */ //subgroup A: double m_left, m_right; //subgroup B: double mp_front_left, mp_front_right, mp_back_left, mp_back_right; //mp = motor perpendicular stabilizing_motors_force_to_pwm ( // this calculates the pwms for yaw motors // These are actually switched for SubZero 0.5*Yaw_Force_Needed - Forward_Force_Needed, // m_left //we might change hard-coded 0.5 ratio later -0.5*Yaw_Force_Needed - Forward_Force_Needed, // m_right 0, 0// m_back left, back right (not used) //0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_front_left //-0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_front_right //0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_back_left //-0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_back_right &m_left, //assumptions: polarity of motors: + pointed towards front O-ring &m_right, //positive yaw is left turn NULL,//&m_back_left, //positive foward is forward NULL//&m_back_right //most importantly, assume motors work like rudders kicking backwards ); #ifdef PRIORITIZE_PITCH_OVER_DEPTH if (ABS(current_orientation.pitch) > 30) { Depth_Force_Needed *= 0.8; } else if (ABS(current_orientation.pitch) > 35) { Depth_Force_Needed *= 0.6; } else if (ABS(current_orientation.pitch) > 40) { Depth_Force_Needed *= 0.3; } #endif stabilizing_motors_force_to_pwm ( // this calculates the pwms for pitch and roll motors //0.5*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed, // m_front_left //-0.5*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed, // m_front_right //-0.5*Pitch_Force_Needed + 0.4*Depth_Force_Needed, // m_rear 0.25*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_front_left -0.25*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_front_right 0.25*Roll_Force_Needed - 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_back_left -0.25*Roll_Force_Needed - 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_back_right &mp_front_left, //assumptions: polarity of motors: + pointed towards water surface &mp_front_right, //positive roll is CCW if we are looking at the back O-ring of the submarine &mp_back_left, //positive depth is deeper &mp_back_right //positive pitch is nose dive ); M_LEFT = (int)m_left; M_RIGHT = (int)m_right; //M_BACK_LEFT = (int)m_back_left; //M_BACK_RIGHT = (int)m_back_right; MP_FRONT_LEFT = (int)mp_front_left; MP_FRONT_RIGHT = (int)mp_front_right; MP_BACK_LEFT = (int)mp_pback_left; MP_BACK_RIGHT = (int)mp_back_right; /** Note that motor_force_to_pwm returns a value between -400 and 400, and the factors are such that the sum of * each factor for every motor adds up (absolutely) to 1.0. Physics son! */ // write the motor settings int i; for ( i = 0; i < NUM_MOTORS; i++ ) { set_motor_duty_cycle(i, motor_duty_cycle[i]); } }