You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
143 lines
3.6 KiB
C
143 lines
3.6 KiB
C
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
#include "taenzer.h"
|
|
#include "avrIOhelper/io-helper.h"
|
|
#include "kraftsensor.h"
|
|
#include "notaus.h"
|
|
#include "pid_controller.h"
|
|
#include "common.h"
|
|
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
#define TAENZER_KRAFT_SETPOINT 12000
|
|
#define TAENZER_KRAFT_HYST 500
|
|
|
|
|
|
taenzer_state_t taenzer_state;
|
|
PID_VARS_INIT(regler);
|
|
|
|
taenzer_state_t taenzer_state = {
|
|
.homed = 0,
|
|
.pos = 0,
|
|
.force_setpoint = TAENZER_KRAFT_SETPOINT,
|
|
.active = 0
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
extern void send_settings(void);
|
|
void do_taenzer(){
|
|
|
|
/* Homing */
|
|
if(read_Input(IN_TAENZER_HOME, LEVEL)){
|
|
TCCR4B &= ~_BV(CS41); //TURN OFF
|
|
taenzer_state.homed = 1;
|
|
taenzer_state.pos = 0;
|
|
}
|
|
if(!taenzer_state.homed && notaus_state == POWER_ON){
|
|
ICR4 = 30;
|
|
OCR4A = 15;
|
|
set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
|
|
TCCR4B |= _BV(CS41); //TURN ON
|
|
}
|
|
|
|
if (read_Input(BTN_KRAFT_PLUS, RISING)) {
|
|
taenzer_state.force_setpoint += 1000;
|
|
#if PLC_MQTT_ENABLED
|
|
send_settings();
|
|
#endif
|
|
if(taenzer_state.homed == 1 && read_Input(BTN_KRAFT_MINUS, LEVEL))
|
|
{
|
|
kraftsensor_zero_offset = -(kraftsensor_value-kraftsensor_zero_offset);
|
|
}
|
|
}
|
|
if (read_Input(BTN_KRAFT_MINUS, RISING)) {
|
|
taenzer_state.force_setpoint -= 1000;
|
|
#if PLC_MQTT_ENABLED
|
|
send_settings();
|
|
#endif
|
|
if(taenzer_state.homed == 1 && read_Input(BTN_KRAFT_PLUS, LEVEL))
|
|
{
|
|
kraftsensor_zero_offset = -(kraftsensor_value-kraftsensor_zero_offset);
|
|
}
|
|
}
|
|
if (read_Input(BTN_TAENZER_START, RISING)) {
|
|
taenzer_state.active = 1;
|
|
}
|
|
|
|
/* Force regualtion */
|
|
if(kraftsensor_valid && taenzer_state.active && taenzer_state.homed && taenzer_state.pos >= 0){
|
|
int32_t err = (kraftsensor_value - taenzer_state.force_setpoint);
|
|
|
|
double pid_out = pid(®ler, err);
|
|
int32_t out = (int32_t)pid_out;
|
|
|
|
ICR4 = 400000/abs(out);
|
|
OCR4A = ICR4/2;
|
|
|
|
if(kraftsensor_valid && notaus_state == POWER_ON){
|
|
if(out < -TAENZER_KRAFT_HYST/2 && taenzer_state.pos < 970000UL){
|
|
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
|
|
}
|
|
//else
|
|
// TCCR4B &= ~_BV(CS41); //TURN OFF
|
|
}
|
|
|
|
void timer4_init()
|
|
{
|
|
TCCR4A |= (1<<COM4A1);
|
|
TCCR4B |= _BV(WGM43);
|
|
|
|
TIMSK4 |= 1<<TOIE4;
|
|
// TIMSK4 |= 1<<OCIE4A;
|
|
|
|
ICR4 = 30;
|
|
OCR4A = 15;
|
|
|
|
DDRH |= 1 << 3;
|
|
}
|
|
|
|
ISR(TIMER4_OVF_vect) {
|
|
if(ioHelperReadBit(outStates, MOTOR_TAENZER_DIR))
|
|
taenzer_state.pos -= 1;
|
|
else
|
|
taenzer_state.pos += 1;
|
|
}
|
|
|
|
//ISR(TIMER4_OVF_vect) {
|
|
//}
|