add prints

still needs some tuning

abzug.c

@@ -1,5 +1,7 @@
 #include <avr/io.h>
+#include <stdint.h>
 #include "avrIOhelper/io-helper.h"
+#include "common.h"
 
 uint16_t abzug_speed = 100;
 
@@ -14,8 +16,11 @@ void timer3_init()
     DDRE |= 1 << 4;
 }
 
+uint8_t get_abzug_state(){
+    return TCCR3B & _BV(CS31);
+}
 
-void send_settings(void);
+extern void send_settings(void);
 void do_abzug(){
 
     if (read_Input(BTN_ABZUG_EIN, RISING)) {
@@ -35,26 +40,6 @@ void do_abzug(){
     }
 
     if (read_Input(BTN_ABZUG_PLUS, RISING)) {
-        if(abzug_speed <= 900)
-            abzug_speed += 100;
-        else
-            abzug_speed = 1000;
-#if PLC_MQTT_ENABLED
-        send_settings();
-#endif
-    }
-
-    if (read_Input(BTN_ABZUG_MINUS, RISING)) {
-        if(abzug_speed >= 110)
-            abzug_speed -= 100;
-        else
-            abzug_speed = 10;
-#if PLC_MQTT_ENABLED
-        send_settings();
-#endif
-    }
-
-    if (read_Input(BTN_ABZUG_PLUS_FEIN, RISING)) {
         if(abzug_speed <= 990)
             abzug_speed += 10;
         else
@@ -64,7 +49,7 @@ void do_abzug(){
 #endif
     }
 
-    if (read_Input(BTN_ABZUG_MINUS_FEIN, RISING)) {
+    if (read_Input(BTN_ABZUG_MINUS, RISING)) {
         if(abzug_speed >= 20)
             abzug_speed -= 10;
         else
@@ -74,6 +59,26 @@ void do_abzug(){
 #endif
     }
 
+    if (read_Input(BTN_ABZUG_PLUS_FEIN, RISING)) {
+        if(abzug_speed <= 999)
+            abzug_speed += 1;
+        else
+            abzug_speed = 1000;
+#if PLC_MQTT_ENABLED
+        send_settings();
+#endif
+    }
+
+    if (read_Input(BTN_ABZUG_MINUS_FEIN, RISING)) {
+        if(abzug_speed >= 11)
+            abzug_speed -= 1;
+        else
+            abzug_speed = 10;
+#if PLC_MQTT_ENABLED
+        send_settings();
+#endif
+    }
+
     // 16000000/8
 
     ICR3 = ((1.0/abzug_speed)*3.14*42.0*(1.0/5.0))*2.0*500.0*(1/1.03);

abzug.h

@@ -7,5 +7,6 @@ extern uint16_t abzug_speed;
 
 void do_abzug(void);
 void timer3_init(void);
+uint8_t get_abzug_state(void);
 
 #endif

avrIOhelper/io-helper.h

@@ -154,4 +154,6 @@ void ioHelperEdgeDetector(void);
 #define IN_TAENZER_HOME         BitPinF0
 #define IN_SPULE_HOME           BitPinF1
 
+#define IN_BREMSE_STATE         BitPinF2
+
 #endif

kraftsensor.c

@@ -12,6 +12,8 @@
 int32_t kraftsensor_value;
 uint8_t kraftsensor_valid;
 
+int32_t kraftsensor_zero_offset = 0/*197700*/;
+
 void timer2_init()
 {
     TCCR2A = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
@@ -27,47 +29,25 @@ void kraftsensor_init(){
 }
 
 void do_kraftsensor(){
-    static int32_t last_read;
-    static int32_t old_value;
     uint16_t m_data[4];
-    int32_t kraftsensor_read;
 
     /* read 2 16bit values and merge to 32bit signed integer */
     readReg(1,0,2);
-    if(wait_receive(2, m_data, 100)){
+    if(wait_receive(2, m_data, 10)){
         kraftsensor_valid = 0;
-        //printf("modbus error\n\r");
+        printf("modbus error\n\r");
     }
     else{
         kraftsensor_valid = 1;
         int32_t tmp = (uint32_t)m_data[1]<<16;
         tmp |= m_data[0];
 
-        //kraftsensor_value = tmp;
         /* conversion magic to milliNewton */
-        kraftsensor_read = ((tmp + 197700 /*539363*/)*9.81)/177.380;
+        kraftsensor_value = (((tmp /* + 539363*/)*9.81)/177.380)+kraftsensor_zero_offset;
-
-        if(abs(kraftsensor_read - old_value) > 10000){
-            if(abs(last_read - kraftsensor_read) > 10000){
-                kraftsensor_value = old_value;
-                //printf("delta: %ld\tvalue:%ld\n", kraftsensor_read - old_value, kraftsensor_read);
-                //printf("spike\n");
     }
-            else{
-                kraftsensor_value = kraftsensor_read;
-                //printf("jump\n");
-            }
-        }
-        else{
-            kraftsensor_value = kraftsensor_read;
-        }
-        last_read = kraftsensor_read;
-        old_value = kraftsensor_value;
-    }
-
 }
 
-uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
 
     uint8_t breaker = timeout;
     while(!receiveOkay && breaker) { //wait for client response, time out after 1s
@@ -92,6 +72,23 @@ uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
     return 0;
 }
 
+int8_t wait_write(uint8_t timeout){
+    uint8_t breaker = timeout;
+    while(!receiveOkay && breaker) { //wait for client response, time out after 1s
+        breaker--;
+        _delay_ms(1);
+        if(breaker==0)
+            return -1;
+    }
+
+    if(receiveOkay) { //if this fails, there was either no response or a crc error
+        if(rxbuffer[1]&0x80) { //client responded with an error code
+            return rxbuffer[1]&0x80;
+        }
+    }
+    return 0;
+}
+
 void readReg(uint8_t slaveid, uint16_t address, uint8_t amount) {
     _delay_ms(2);
     rxbuffer[0]=slaveid;

kraftsensor.h

@@ -8,10 +8,12 @@
 extern int32_t kraftsensor_value;
 extern uint8_t kraftsensor_valid;
 
+extern int32_t kraftsensor_zero_offset;
+
 void timer2_init();
 void kraftsensor_init();
 void do_kraftsensor(void);
-uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
 void readReg(uint8_t slaveid, uint16_t address, uint8_t amount);
 void writeReg(uint8_t slaveid, uint16_t address, uint16_t value);
 

main.c

@@ -2,6 +2,7 @@
 #include <avr/interrupt.h>
 #include <avr/wdt.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
 
@@ -89,6 +90,12 @@ void send_values(void){
 
     itoa((250*60)/ICR5, msg, 10);
     mqtt_pub(&mqtt_client, "/Filamentanlage/05_Abzug/state/spule/speed", msg, strlen(msg));
+
+    sprintf(msg, "%d", windings);
+    mqtt_pub(&mqtt_client, "/Filamentanlage/05_Abzug/state/spule/windings", msg, strlen(msg));
+
+    sprintf(msg, "%ld", spule_trans_pos);
+    mqtt_pub(&mqtt_client, "/Filamentanlage/05_Abzug/state/spule/trans_pos", msg, strlen(msg));
 }
 
 // send settings wich only change on buttion press
@@ -96,8 +103,6 @@ void send_settings(void){
     //TODO only send on change or improve performance otherwise
     char msg[10];
 
-    //PORTH &= ~(1<<5);
-    //PORTH |= (1<<5);
     /* Abzug */
     sprintf(msg, "%d", abzug_speed);
     mqtt_pub(&mqtt_client, "/Filamentanlage/05_Abzug/state/abzug/speed", msg, strlen(msg));
@@ -196,18 +201,15 @@ int main()
 
     set_Output(LED_FEHLER, OFF);
 
-    set_Output(BitPH5, ON);
+    set_Output(BitPH5, ON); //DEBUG
-    
-
-    while(1)
-    {
-        wdt_reset(); // WDT reset at least every sec
     
 #if PLC_MQTT_ENABLED
-        if(millis() < 10)
     send_settings();
 #endif
 
+    while(1){
+        wdt_reset(); // WDT reset at least every sec
+
         ioHelperReadPins();
         ioHelperDebounce();
         ioHelperEdgeDetector();
@@ -223,7 +225,7 @@ int main()
         }
 
         if(millis() - timer_modbus_poll > 20){
-            do_kraftsensor();
+            do_kraftsensor();   // 8ms !!!
             timer_modbus_poll += 20;
         }
 
@@ -231,7 +233,7 @@ int main()
         // send misc info
         if(millis() - timer_send_info > 200){
             timer_send_info += 200;
-            send_values();
+            send_values(); // 10ms
             //send_info(); // 27ms every 200ms
         }
 #endif

pid_controller.h

@@ -25,8 +25,8 @@ typedef struct {
 } PID_vars;
 
 
-#define PID_VARS_INIT(x) PID_vars x = {.Kp=1.5,.Ki=0.00,.Kd=0.0,.output_max=20000.0, \
+#define PID_VARS_INIT(x) PID_vars x = {.Kp=1.3,.Ki=0.00,.Kd=0.00,.output_max=25000.0, \
-                       .output_min=-20000.0,._integral_sum=0.0,._prev_err=0.0,._dt=1.0}
+                       .output_min=-25000.0,._integral_sum=0.0,._prev_err=0.0,._dt=1.0}
 
 /* Function Prototypes */
 double pid(PID_vars *vars, double current_err);

spule.c

@@ -10,10 +10,12 @@
 volatile uint16_t windings = 0;
 volatile uint16_t windings_wakeup = 0;
 
+volatile uint8_t trans_state = 0;
+
 int32_t spule_trans_pos = 0;
 uint8_t spule_trans_homed = 0;
 
-#define TRANS_ROT_FACTOR 0.07
+#define TRANS_ROT_FACTOR 0.14
 
 void timer1_init()
 {
@@ -50,8 +52,16 @@ static void spule_onoff(uint8_t state){
     }
 }
 
+void set_spooling_speed(uint16_t speed){
+    ICR5=speed;
+    OCR5C = ICR5/2;
+
+    ICR1 = ICR5/TRANS_ROT_FACTOR;
+    OCR1A = ICR1/2;
+}
+
 void do_spule(){
-    //PORTH &= ~(1<<5);
+    // Translatoric axis homeing code
     if(read_Input(IN_SPULE_HOME, LEVEL) && spule_trans_homed == 0){
         spule_trans_homed = 1;
         spule_trans_pos = 0;
@@ -63,40 +73,66 @@ void do_spule(){
         OCR1A = ICR1/2;
     }
 
+    // if not homed goto home
     if(!spule_trans_homed){
         ICR1 = 100;
         OCR1A = 50;
         set_Output(MOTOR_TRANS_DIR, 1); // direction: front
         TCCR1B |= _BV(CS11); //TURN ON
     }
-    else if(taenzer_state.pos <= 10){
+
+    // manual forwarding if button is held
+    else if(!get_abzug_state() || (get_abzug_state() && read_Input(IN_BREMSE_STATE, LEVEL)) ){
+        if(read_Input(BTN_WICKELN_EIN, LEVEL)){
+            set_spooling_speed(300);
+            spule_onoff(1);
+        }
+        else
             spule_onoff(0);
     }
+
+    // normal operation
     else{
         /* speed regulation - keep taenzer at 10% */
-        int32_t tmp = 0;//(100 - (int32_t)taenzer_state.pos/1000);
+        float p = 100.0/(int32_t)(taenzer_state.pos/1000);
+        p-=1;
+        p/=2;
+        p+=1;
+        //tmp = (int32_t)(taenzer_state.pos/10000);
+        //printf("%ld\n", tmp);
 
         //printf("temp1: %d\n", tmp);
         //TODO fix bounds
         //if(tmp < -7500/abzug_speed/2)
         //    tmp = -7500/abzug_speed/2;
         //printf("temp2: %d\n", tmp);
-        if(tmp < -35)
+        if(p < 0.5)
-            tmp = -35;
+            p = 0.5;
+        if(p > 2)
+            p = 2;
 
-        ICR5=7500/abzug_speed + tmp;
+        uint16_t base_speed = (14000/abzug_speed);
+        uint16_t ctrl_speed = base_speed * p;
+
+        if(ctrl_speed <= 70)
+            ctrl_speed = 70;
+
+        ICR5 = ctrl_speed;
         OCR5C = ICR5/2;
 
+        if(trans_state != 4)
             ICR1 = ICR5/TRANS_ROT_FACTOR;
+        else
+            ICR1 = 0.5*(ICR5/TRANS_ROT_FACTOR);
         OCR1A = ICR1/2;
 
-        if (read_Input(BTN_WICKELN_EIN, RISING)) {
+        if (read_Input(BTN_WICKELN_EIN, RISING) && !read_Input(IN_BREMSE_STATE, LEVEL)) {
             spule_onoff(1);
         }
         if (read_Input(BTN_WICKELN_AUS, RISING)) {
             spule_onoff(0);
         }
-
+    }
     if (read_Input(BTN_INIT, RISING)) {
         spule_trans_homed = 0;
         taenzer_state.homed = 0;
@@ -104,7 +140,17 @@ void do_spule(){
         windings = 0;
         windings_wakeup = 0;
     }
+
+    if (read_Input(IN_BREMSE_STATE, FALLING)) {
+        printf("draußen\n");
+        spule_onoff(1);
     }
+
+    if (read_Input(IN_BREMSE_STATE, RISING)) {
+        printf("drinne\n");
+        spule_onoff(0);
+    }
+
     //PORTH |= (1<<5);
 }
 
@@ -125,10 +171,12 @@ ISR(TIMER5_OVF_vect) {
     //PORTH &= ~(1<<5);
     static uint16_t steps = 0;
     steps++;
-    if(steps == 50000){
+    if(steps == 25000){
         windings++;
         steps=0;
-        printf("windungen: %d\n", windings);
+        printf("windungen: %d\t", windings);
+        printf("trans pos: %ld\n", spule_trans_pos);
+        printf("speed %d\n", ICR1);
     }
 
     if(windings == windings_wakeup){
@@ -138,23 +186,30 @@ ISR(TIMER5_OVF_vect) {
     uint8_t windings_on_layer = windings % 25;
 
     if(windings_on_layer == 0 && steps == 0){
+        trans_state = 1;
         ICR1 = ICR5/TRANS_ROT_FACTOR;
         OCR1A = ICR1/2;
         set_Output(MOTOR_TRANS_DIR, TOGGLE);
         printf("toggle at pos: %ld\n", spule_trans_pos);
+        printf("speed %d\n", ICR1);
     }
     if(windings_on_layer == 1 && steps == 0){
+        trans_state = 2;
         printf("nachlauf aufbauen\n");
         TCCR1B &= ~(_BV(CS11));
     }
     if(windings_on_layer == 3 && steps == 0){
+        trans_state = 3;
         TCCR1B |= _BV(CS11);
         printf("done\n");
+        printf("speed %d\n", ICR1);
     }
     if(windings_on_layer == 21 && steps == 0){
+        trans_state = 4;
         ICR1 = 0.5*(ICR5/TRANS_ROT_FACTOR);
         OCR1A = ICR1/2;
         printf("nachlauf abbauen\n");
+        printf("speed %d\n", ICR1);
     }
-    //PORTH |= (1<<5);
+    //;PORTH |= (1<<5);
 }

spule.h

@@ -1,8 +1,13 @@
 #ifndef _SPULE_H_
 #define _SPULE_H_
 
+#include <stdint.h>
+
 void timer1_init(void);
 void timer5_init(void);
 void do_spule(void);
 
+extern volatile uint16_t windings;
+extern int32_t spule_trans_pos;
+
 #endif

taenzer.c

@@ -5,13 +5,14 @@
 #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 1000
+#define TAENZER_KRAFT_HYST 500
 
 
 taenzer_state_t taenzer_state;
@@ -46,7 +47,7 @@ double pid(PID_vars *vars, double current_err) {
   return output;
 }
 
-void send_info(void);
+extern void send_settings(void);
 void do_taenzer(){
 
     /* Homing */
@@ -65,31 +66,39 @@ void do_taenzer(){
     if (read_Input(BTN_KRAFT_PLUS, RISING)) {
         taenzer_state.force_setpoint += 1000;
 #if PLC_MQTT_ENABLED
-        send_info();
+        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_info();
+        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 && taenzer_state.pos < 500000){
+    if(kraftsensor_valid && taenzer_state.active && taenzer_state.homed && taenzer_state.pos >= 0){
-        int16_t err = (kraftsensor_value - taenzer_state.force_setpoint);
+        int32_t err = (kraftsensor_value - taenzer_state.force_setpoint);
 
         double pid_out = pid(&regler, err);
-        int16_t out = (int16_t)pid_out;
+        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 < 300000UL*/){
+            if(out < -TAENZER_KRAFT_HYST/2  && taenzer_state.pos < 970000UL){
                 set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
                 TCCR4B |= _BV(CS41); //TURN ON
             }
@@ -97,12 +106,15 @@ void do_taenzer(){
                 set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
                 TCCR4B |= _BV(CS41); //TURN ON
             }
-            else
+            else{
                 TCCR4B &= ~_BV(CS41); //TURN OFF
             }
+        }
         else
             TCCR4B &= ~_BV(CS41); //TURN OFF
     }
+    //else
+    //    TCCR4B &= ~_BV(CS41); //TURN OFF
 }
 
 void timer4_init()