void PIC_Motor_PID_Loop()
{
/*Motor A PID*/
if(MotorA_Position==motorATargetPos)
{
PIC_Motor_STOP(MOTOR_A);
MA_Status=1;
}
else
{
motorA_pos=MotorA_Position;
motorA_setpoint=motorATargetPos;
PID_PID(&MotorA_PID, &motorA_pos, &MOTOR_A_PID_OUTPUT, &motorA_setpoint, MOTOR_PID_KP, MOTOR_PID_KI, MOTOR_PID_KD, DIRECT);
PID_Compute(&MotorA_PID);
if(MOTOR_A_PID_OUTPUT==0)
{
PIC_Motor_STOP(MOTOR_A);
}
else if(MOTOR_A_PID_OUTPUT>0)
{
PIC_Motor_FW(MOTOR_A);
}
else
{
PIC_Motor_BW(MOTOR_A);
MOTOR_A_PID_OUTPUT = MOTOR_A_PID_OUTPUT*(-1);
}
CCPR1L=(unsigned char)(MOTOR_A_PID_OUTPUT);
}
/*Motor B PID*/
if(MotorB_Position==motorBTargetPos)
{
PIC_Motor_STOP(MOTOR_B);
MB_Status=1;
}
else
{
motorB_pos=MotorB_Position;
motorB_setpoint=motorBTargetPos;
PID_PID(&MotorB_PID, &motorB_pos, &MOTOR_B_PID_OUTPUT, &motorB_setpoint, MOTOR_PID_KP, MOTOR_PID_KI, MOTOR_PID_KD, DIRECT);
PID_Compute(&MotorB_PID);
if(MOTOR_B_PID_OUTPUT==0)
{
PIC_Motor_STOP(MOTOR_B);
}
else if(MOTOR_B_PID_OUTPUT>0)
{
PIC_Motor_FW(MOTOR_B);
}
else
{
PIC_Motor_BW(MOTOR_B);
MOTOR_B_PID_OUTPUT = MOTOR_B_PID_OUTPUT*(-1);
}
CCPR2L=(unsigned char)(MOTOR_B_PID_OUTPUT);
}
}
// input : deg/s
// rate_type : 1-> earth frame desire rate(roll, pitch, yaw)
// rate_type : 0-> body frame desire rate(p,q,r)
void rate_controller(float* desire_rate, unsigned long rate_type, float hover){
_r_rate_PID.myInput = _w_gyro[0]*57.29577951; // deg/s
_p_rate_PID.myInput = _w_gyro[1]*57.29577951;
_y_rate_PID.myInput = _w_gyro[2]*57.29577951;
if(rate_type == 1){
// earth frame to body frame
_r_rate_PID.mySetpoint = desire_rate[0] - sinf(_euler_angle[1])*desire_rate[2];
_p_rate_PID.mySetpoint = cosf(_euler_angle[0])*desire_rate[1] + sinf(_euler_angle[0])*cosf(_euler_angle[1])*desire_rate[2];
_y_rate_PID.mySetpoint = -sinf(_euler_angle[1])*desire_rate[1] + cosf(_euler_angle[0])*sinf(_euler_angle[1])*desire_rate[2];
}else{
_r_rate_PID.mySetpoint = desire_rate[0];
_p_rate_PID.mySetpoint = desire_rate[1];
_y_rate_PID.mySetpoint = desire_rate[2];
}
PID_Compute(&_r_rate_PID);
PID_Compute(&_p_rate_PID);
PID_Compute(&_y_rate_PID);
_r_rate_out = _r_rate_PID.myOutput;
_p_rate_out = _p_rate_PID.myOutput;
_y_rate_out = _y_rate_PID.myOutput;
_front_out = hover*front_factor - _p_rate_out - _y_rate_out;
_right_out = hover*right_factor - _r_rate_out + _y_rate_out;
_back_out = hover*back_factor + _p_rate_out - _y_rate_out;
_left_out = hover*left_factor + _r_rate_out + _y_rate_out;
_front_out = constraints(_front_out,7, 60);
_right_out = constraints(_right_out,7, 60);
_back_out = constraints(_back_out,7, 60);
_left_out = constraints(_left_out,7, 60);
Motor_write(front_motor, _front_out);
Motor_write(right_motor, _right_out);
Motor_write(back_motor, _back_out);
Motor_write(left_motor, _left_out);
}
void Callback_20ms() {
desiredState.key.abs.pos.z = altitudeValue / 1000.0;
/* Setting motor speed based on altitude */
lcd_buffer_print(LCD_LINE4, "In: %1.3f", sonarDistance);
outValue = PID_Compute(sonarDistance, desiredState.key.abs.pos.z, &z_axis_PID);
desiredState.key.avg_motor_us = map(outValue * map(analogRead, 0, 4096, 0, 1), 0, 0.5, MOTOR_MIN_US, MOTOR_MAX_US);
lcd_buffer_print(LCD_LINE5, "Mot: %4.0f", desiredState.key.avg_motor_us);
//lcd_buffer_print(LCD_LINE7, "Diff: %3.0f", desiredState.key.motor_diff_us);
/* Obtain accelerometer an gyro values */
Get_Gyro_Rates(¤tState.key.gyro.vel.x, ¤tState.key.gyro.vel.y, ¤tState.key.gyro.vel.z);
Get_Accel_Angles(¤tState.key.accel.pos.x, ¤tState.key.accel.pos.y);
//Get_Mag_Value_Normalized(¤tState.key.magn.pos.x, ¤tState.key.magn.pos.y, ¤tState.key.magn.pos.z);
Get_Accel_Gravity_Power(¤tState.key.accel.vel.x, ¤tState.key.accel.vel.y, ¤tState.key.accel.vel.z);
get_Angle_AHRS(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.acc.x, ¤tState.key.Kalman.acc.y, ¤tState.key.Kalman.acc.z);
// get_Angle_AHRS_Mahony(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.vel.x, ¤tState.key.Kalman.vel.y, ¤tState.key.Kalman.vel.z);
/* Calcolo Roll e Pitch con il filtro di Kalman */
currentState.key.Kalman.pos.x = Compute_AVG(getAngle(currentState.key.accel.pos.x, currentState.key.gyro.vel.x, dt, rollKalman), &rollAVG);
currentState.key.Kalman.pos.y = Compute_AVG(getAngle(currentState.key.accel.pos.y, currentState.key.gyro.vel.y, dt, pitchKalman), &pitchAVG);
//currentState.key.Kalman.pos.z = currentState.key.Kalman.acc.z;
// Angolo di Yaw mediante giroscopio
// currentState.key.Kalman.pos.z = currentState.key.gyro.vel.z;
// Get_Mag_Heading_Compensated (¤tState.key.Kalman.pos.z, currentState.key.Kalman.pos.x, currentState.key.Kalman.pos.y);
// Get_Mag_Heading(¤tState.key.Kalman.acc.z);
desiredState.key.x_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.x, 0, &Roll_PID), -30, 30, 60, 120);
desiredState.key.y_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.y, 0, &Pitch_PID), -30, 30, 60, 120);
currentState.key.Kalman.pos.z += currentState.key.gyro.vel.z*dt;
Motor_Write_us(MOTOR_UPPER, desiredState.key.avg_motor_us + desiredState.key.motor_diff_us);
Motor_Write_us(MOTOR_BOTTOM, desiredState.key.avg_motor_us - desiredState.key.motor_diff_us);
Servo_Write_deg(SERVO_ROLL, desiredState.key.x_servo_deg);
Servo_Write_deg(SERVO_PITCH, desiredState.key.y_servo_deg); // pitch servo is trimmed 18°
}
int32_t Reflow_Run(uint32_t thetime, float meastemp, uint8_t* pheat, uint8_t* pfan, int32_t manualsetpoint) {
int32_t retval=0;
if(manualsetpoint) {
PID.mySetpoint = (float)manualsetpoint;
} else { // Figure out what setpoint to use from the profile, brute-force way. Fix this.
uint8_t idx = thetime / 10;
uint16_t start = idx * 10;
uint16_t offset = thetime-start;
if(idx<47) {
uint32_t value = profiles[profileidx]->temperatures[idx];
if(value>0) {
uint32_t value2 = profiles[profileidx]->temperatures[idx+1];
uint32_t avg = (value*(10-offset) + value2*offset)/10;
printf(" setpoint %uC",avg);
intsetpoint = avg; // Keep this for UI
PID.mySetpoint = (float)avg;
} else {
retval = -1;
}
} else {
retval = -1;
}
}
if(!manualsetpoint) {
// Plot actual temperature on top of desired profile
int realx = (thetime / 5) + XAXIS;
int y=(uint16_t)(meastemp * 0.2f);
y = YAXIS-y;
LCD_SetPixel(realx,y);
}
PID.myInput=meastemp;
PID_Compute(&PID);
uint32_t out = PID.myOutput;
if(out<248) { // Fan in reverse
*pfan=255-out;
*pheat=0;
} else {
*pheat=out-248;
if(*pheat>192) *pfan=2; else *pfan=4; // When heating like crazy make sure we can reach our setpoint
}
return retval;
}
///----------------------------------------------------------------------------
///
/// \brief main
/// \return -
/// \remarks -
///
///----------------------------------------------------------------------------
int32_t main(void)
{
uint16_t j;
Test_Pid.fGain = 1.0f; //
Test_Pid.fMin = -1.0f; //
Test_Pid.fMax = 1.0f; //
/* Proportional only gain */
Test_Pid.fKp = 1.0f; // init gains
Test_Pid.fKi = 0.0f; //
Test_Pid.fKd = 0.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.0f;
for (j = 0; j < KP_ONLY_CYCLES; j++) { // settle
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
/* Integral only gain */
Test_Pid.fKp = 0.0f; // init gains
Test_Pid.fKi = 1.0f; //
Test_Pid.fKd = 0.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
/* Derivative only gain */
Test_Pid.fKp = 0.0f; // init gains
Test_Pid.fKi = 0.0f; //
Test_Pid.fKd = 1.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.0f;
for (j = 0; j < KD_ONLY_CYCLES; j++) { // settle
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
while (1) {
}
}
