|
|
|
|
@ -21,6 +21,7 @@ taenzer_state_t taenzer_state = {
|
|
|
|
|
.homed = 0,
|
|
|
|
|
.pos = 0,
|
|
|
|
|
.force_setpoint = TAENZER_KRAFT_SETPOINT,
|
|
|
|
|
.active = 0
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
double pid(PID_vars *vars, double current_err) {
|
|
|
|
|
@ -71,9 +72,12 @@ void do_taenzer(){
|
|
|
|
|
send_info();
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
if (read_Input(BTN_TAENZER_START, RISING)) {
|
|
|
|
|
taenzer_state.active = 1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Force regualtion */
|
|
|
|
|
if(kraftsensor_valid && taenzer_state.homed && taenzer_state.pos >= 0 && taenzer_state.pos < 500000){
|
|
|
|
|
if(kraftsensor_valid && taenzer_state.active && taenzer_state.homed && taenzer_state.pos >= 0 && taenzer_state.pos < 500000){
|
|
|
|
|
int16_t err = (kraftsensor_value - taenzer_state.force_setpoint);
|
|
|
|
|
|
|
|
|
|
double pid_out = pid(®ler, err);
|
|
|
|
|
|