Filamentstr/05_wickler
Archived
1
0
Fork 0
Code Issues 3 Pull Requests Releases Wiki Activity

add pid controller

Browse Source
This commit is contained in:
Eggert Jung
2022-04-04 15:39:54 +02:00
parent eb680bac89
commit 836d2e3e6e
2 changed files with 75 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
pid_controller.h Normal file
View File

@@ -0,0 +1,34 @@
/*
* pid_controller.h
*
* Created on: Apr 24, 2016
* Author: subham roy
*/
#ifndef __PID_CONTROLLER_H
#define __PID_CONTROLLER_H
typedef struct {
/* PID controller parameters */
double Kp;
double Ki;
double Kd;
/* max output limits for the PID controller */
double output_max;
double output_min;
/* below are session variables for the PID controller */
double _integral_sum;
double _prev_err;
double _dt;
} PID_vars;
#define PID_VARS_INIT(x) PID_vars x = {.Kp=1.0,.Ki=0.00,.Kd=0.1,.output_max=15000.0, \
.output_min=-15000.0,._integral_sum=0.0,._prev_err=0.0,._dt=1.0}
/* Function Prototypes */
double pid(PID_vars *vars, double current_err);
#endif

51
taenzer.c
View File

@@ -3,7 +3,11 @@
#include "taenzer.h"
#include "avrIOhelper/io-helper.h"
#include "kraftsensor.h"
#include "notaus.h"
#include "pid_controller.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#define TAENZER_KRAFT_SETPOINT 12000
@@ -11,6 +15,7 @@
taenzer_state_t taenzer_state;
PID_VARS_INIT(regler);
taenzer_state_t taenzer_state = {
.homed = 0,
@@ -18,6 +23,28 @@ taenzer_state_t taenzer_state = {
.force_setpoint = TAENZER_KRAFT_SETPOINT,
};
double pid(PID_vars *vars, double current_err) {
/* current_error = setpoint - current_process_variable */
vars->_integral_sum += current_err*(vars->_dt);
double output = (vars->Kp)*current_err \
+ (vars->Ki)*(vars->_integral_sum) \
+ (vars->Kd)*((current_err-(vars->_prev_err)) \
/(vars->_dt));
vars->_prev_err = current_err;
/* limit output within output_min and output_max */
if (output>(vars->output_max))
output = vars->output_max;
else if (output<(vars->output_min))
output = vars->output_min;
return output;
}
void send_info(void);
void do_taenzer(){
@@ -49,19 +76,23 @@ void do_taenzer(){
if(kraftsensor_valid && taenzer_state.homed && taenzer_state.pos >= 0 && taenzer_state.pos < 500000){
int16_t err = (kraftsensor_value - taenzer_state.force_setpoint);
if(err > 8000)
err=8000;
double pid_out = pid(&regler, err);
int16_t out = (int16_t)pid_out;
ICR4 = 400000/err;
ICR4 = 400000/abs(out);
OCR4A = ICR4/2;
if(kraftsensor_value < TAENZER_KRAFT_SETPOINT - (TAENZER_KRAFT_HYST/2)){
set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
TCCR4B |= _BV(CS41); //TURN ON
}
else if(kraftsensor_value > TAENZER_KRAFT_SETPOINT + (TAENZER_KRAFT_HYST/2)){
set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
TCCR4B |= _BV(CS41); //TURN ON
if(kraftsensor_valid && notaus_state == POWER_ON){
if(out < -TAENZER_KRAFT_HYST/2 && taenzer_state.pos < 300000UL){
set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
TCCR4B |= _BV(CS41); //TURN ON
}
else if(out > TAENZER_KRAFT_HYST/2 && taenzer_state.pos > 0){
set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
TCCR4B |= _BV(CS41); //TURN ON
}
else
TCCR4B &= ~_BV(CS41); //TURN OFF
}
else
TCCR4B &= ~_BV(CS41); //TURN OFF