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