From df55dcfe4c1f3e11403b7fd733d46f23571ee793 Mon Sep 17 00:00:00 2001
From: bruno <bruno@brn.systems>
Date: Mon, 5 May 2025 09:02:25 +0200
Subject: [PATCH] test

---
 .gitingnore |   4 +-
 i2c.c       | 136 ++++++------
 i2c.h       |  14 +-
 main.c      | 597 ++++++++++++++++++++++++++++++++--------------------
 4 files changed, 452 insertions(+), 299 deletions(-)

diff --git a/.gitingnore b/.gitingnore
index 17d25bf..672a606 100644
--- a/.gitingnore
+++ b/.gitingnore
@@ -2,4 +2,6 @@ cmake-build-release-avr
 cmake-build-release-avr/*
 cmake-build-*
 build/
-build
\ No newline at end of file
+build
+/build
+/build/
\ No newline at end of file
diff --git a/i2c.c b/i2c.c
index 9a1188e..ee671b6 100644
--- a/i2c.c
+++ b/i2c.c
@@ -1,74 +1,84 @@
+
 /*
  * I2C_Slave_C_File.c
  *
- */ 
-
+ */
 #include "i2c.h"
 
-void I2C_Slave_Init(uint8_t slave_address)
-{
-	TWAR = slave_address;						/* Assign address in TWI address register */
-	TWCR = (1<<TWEN) | (1<<TWEA) | (1<<TWINT);	/* Enable TWI, Enable ack generation, clear TWI interrupt */
+void I2C_Slave_Init(uint8_t slave_address) {
+    TWAR = slave_address << 1; /* Assign address in TWI address register (shifted left by 1) */
+    TWCR = (1<<TWEN) | (1<<TWEA) | (1<<TWIE) | (1<<TWINT); /* Enable TWI, Enable ack generation, enable interrupt, clear interrupt flag */
 }
 
-int8_t I2C_Slave_Listen()
-{
-	while(1)
-	{
-		uint8_t status;							/* Declare variable */
-		while (!(TWCR & (1<<TWINT)));			/* Wait to be addressed */
-		status = TWSR & 0xF8;					/* Read TWI status register with masking lower three bits */
-		if (status == 0x60 || status == 0x68)	/* Check weather own SLA+W received & ack returned (TWEA = 1) */
-		return 0;								/* If yes then return 0 to indicate ack returned */
-		if (status == 0xA8 || status == 0xB0)	/* Check weather own SLA+R received & ack returned (TWEA = 1) */
-		return 1;								/* If yes then return 1 to indicate ack returned */
-		if (status == 0x70 || status == 0x78)	/* Check weather general call received & ack returned (TWEA = 1) */
-		return 2;								/* If yes then return 2 to indicate ack returned */
-		else
-		continue;								/* Else continue */
-	}
+int8_t I2C_Slave_Listen() {
+    while(1) {
+        uint8_t status; /* Declare variable */
+        while (!(TWCR & (1<<TWINT))); /* Wait to be addressed */
+        status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
+        
+        if (status == 0x60 || status == 0x68) /* Check weather own SLA+W received & ack returned (TWEA = 1) */
+            return 0; /* If yes then return 0 to indicate ack returned */
+        
+        if (status == 0xA8 || status == 0xB0) /* Check weather own SLA+R received & ack returned (TWEA = 1) */
+            return 1; /* If yes then return 1 to indicate ack returned */
+        
+        if (status == 0x70 || status == 0x78) /* Check weather general call received & ack returned (TWEA = 1) */
+            return 2; /* If yes then return 2 to indicate ack returned */
+        
+        else
+            continue; /* Else continue */
+    }
 }
 
-int8_t I2C_Slave_Transmit(char data)
-{
-	uint8_t status;
-	TWDR = data;								/* Write data to TWDR to be transmitted */
-	TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);		/* Enable TWI and clear interrupt flag */
-	while (!(TWCR & (1<<TWINT)));				/* Wait until TWI finish its current job (Write operation) */
-	status = TWSR & 0xF8;						/* Read TWI status register with masking lower three bits */
-	if (status == 0xA0)							/* Check weather STOP/REPEATED START received */
-	{
-		TWCR |= (1<<TWINT);						/* If yes then clear interrupt flag & return -1 */
-		return -1;
-	}
-	if (status == 0xB8)							/* Check weather data transmitted & ack received */
-		return 0;									/* If yes then return 0 */
-	if (status == 0xC0)							/* Check weather data transmitted & nack received */
-	{
-		TWCR |= (1<<TWINT);						/* If yes then clear interrupt flag & return -2 */
-		return -2;
-	}
-	if (status == 0xC8)							/* If last data byte transmitted with ack received TWEA = 0 */
-	return -3;									/* If yes then return -3 */
-	else										/* else return -4 */
-	return -4;
+int8_t I2C_Slave_Transmit(char data) {
+    uint8_t status;
+    TWDR = data; /* Write data to TWDR to be transmitted */
+    TWCR = (1<<TWEN) | (1<<TWINT) | (1<<TWEA); /* Enable TWI and clear interrupt flag */
+    
+    while (!(TWCR & (1<<TWINT))); /* Wait until TWI finish its current job (Write operation) */
+    status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
+    
+    if (status == 0xA0) /* Check weather STOP/REPEATED START received */
+    {
+        TWCR |= (1<<TWINT); /* If yes then clear interrupt flag & return -1 */
+        return -1;
+    }
+    
+    if (status == 0xB8) /* Check weather data transmitted & ack received */
+        return 0; /* If yes then return 0 */
+    
+    if (status == 0xC0) /* Check weather data transmitted & nack received */
+    {
+        TWCR |= (1<<TWINT); /* If yes then clear interrupt flag & return -2 */
+        return -2;
+    }
+    
+    if (status == 0xC8) /* If last data byte transmitted with ack received TWEA = 0 */
+        return -3; /* If yes then return -3 */
+    
+    else /* else return -4 */
+        return -4;
 }
 
-char I2C_Slave_Receive()
-{
-	uint8_t status;								/* Declare variable */
-	TWCR=(1<<TWEN)|(1<<TWEA)|(1<<TWINT);		/* Enable TWI, generation of ack and clear interrupt flag */
-	while (!(TWCR & (1<<TWINT)));				/* Wait until TWI finish its current job (read operation) */
-	status = TWSR & 0xF8;						/* Read TWI status register with masking lower three bits */
-	if (status == 0x80 || status == 0x90)		/* Check weather data received & ack returned (TWEA = 1) */
-	return TWDR;								/* If yes then return received data */
-	if (status == 0x88 || status == 0x98)		/* Check weather data received, nack returned and switched to not addressed slave mode */
-	return TWDR;								/* If yes then return received data */
-	if (status == 0xA0)							/* Check weather STOP/REPEATED START received */
-	{
-		TWCR |= (1<<TWINT);						/* If yes then clear interrupt flag & return 0 */
-		return -1;
-	}
-	else
-	return -2;									/* Else return 1 */
-}
+char I2C_Slave_Receive() {
+    uint8_t status; /* Declare variable */
+    TWCR = (1<<TWEN) | (1<<TWEA) | (1<<TWINT); /* Enable TWI, generation of ack and clear interrupt flag */
+    
+    while (!(TWCR & (1<<TWINT))); /* Wait until TWI finish its current job (read operation) */
+    status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
+    
+    if (status == 0x80 || status == 0x90) /* Check weather data received & ack returned (TWEA = 1) */
+        return TWDR; /* If yes then return received data */
+    
+    if (status == 0x88 || status == 0x98) /* Check weather data received, nack returned and switched to not addressed slave mode */
+        return TWDR; /* If yes then return received data */
+    
+    if (status == 0xA0) /* Check weather STOP/REPEATED START received */
+    {
+        TWCR |= (1<<TWINT); /* If yes then clear interrupt flag & return -1 */
+        return -1;
+    }
+    
+    else
+        return -2; /* Else return -2 */
+}
\ No newline at end of file
diff --git a/i2c.h b/i2c.h
index 610e3e0..9b1d947 100644
--- a/i2c.h
+++ b/i2c.h
@@ -1,17 +1,15 @@
 /*
  * I2C_Slave_H_File.h
  *
- */ 
-
-
+ */
 #ifndef I2C_SLAVE_H_FILE_H_
 #define I2C_SLAVE_H_FILE_H_
 
-#include <avr/io.h>								/* Include AVR std. library file */
+#include <avr/io.h> /* Include AVR std. library file */
 
-void I2C_Slave_Init(uint8_t slave_address);		/* I2C slave initialize function with Slave address */
-int8_t I2C_Slave_Listen();						/* I2C slave listen function */
-int8_t I2C_Slave_Transmit(char data);			/* I2C slave transmit function */
-char I2C_Slave_Receive();						/* I2C slave receive function */
+void I2C_Slave_Init(uint8_t slave_address); /* I2C slave initialize function with Slave address */
+int8_t I2C_Slave_Listen(); /* I2C slave listen function */
+int8_t I2C_Slave_Transmit(char data); /* I2C slave transmit function */
+char I2C_Slave_Receive(); /* I2C slave receive function */
 
 #endif /* I2C_SLAVE_H_FILE_H_ */
\ No newline at end of file
diff --git a/main.c b/main.c
index a16a3b7..a5dbf5d 100644
--- a/main.c
+++ b/main.c
@@ -6,6 +6,7 @@
 #include <stdbool.h>
 #include <avr/iom32.h>
 
+// Register Map Definitions
 #define REGISTER_SERVOA_POSITIONH 0
 #define REGISTER_SERVOA_POSITIONL 1
 #define REGISTER_SERVOA_KPA 2
@@ -35,14 +36,13 @@
 #define REGISTER_SERVOB_KDC 26
 #define REGISTER_SERVOB_KDD 27
 
-// Motor A
+// Motor Pins
 #define MOTOR_A_POT 2
 #define MOTOR_A_PIN_A PD5
 #define MOTOR_A_PIN_B PD7
 #define MOTOR_A_PIN_A_OCR OCR1A
 #define MOTOR_A_PIN_B_OCR OCR2
 
-// Motor B
 #define MOTOR_B_POT 3
 #define MOTOR_B_PIN_A PB3
 #define MOTOR_B_PIN_B PD4
@@ -52,15 +52,15 @@
 #define Slave_Address 0x69
 
 // I2C Slave Register Map
-#define REGISTER_COUNT 27
+#define REGISTER_COUNT 28
 volatile uint8_t registers[REGISTER_COUNT];
 
 // I2C State
-volatile uint8_t reg_address = 0;
-volatile bool reg_address_received = false;
+volatile uint8_t reg_pointer = 0;
+volatile bool expecting_address = true;
 
-typedef struct
-{
+// Servo Motor Structure
+typedef struct {
     uint8_t pot_channel;
     volatile uint8_t *pin_a_port;
     volatile uint8_t *pin_a_ddr;
@@ -68,336 +68,479 @@ typedef struct
     volatile uint8_t *pin_b_port;
     volatile uint8_t *pin_b_ddr;
     uint8_t pin_b_bit;
-
-    volatile uint8_t *ocr;
-
     float kp, ki, kd;
     int16_t target;
     int16_t current;
     float integral;
     int16_t last_error;
-
-    bool pot_dir;   // 1 or -1
-    bool motor_dir; // 1 or -1
+    int8_t pot_dir;  // Direction multiplier (1 or -1)
+    int8_t motor_dir; // Direction multiplier (1 or -1)
     volatile void *ocr_a;
     volatile void *ocr_b;
     bool ocr_a_16bit;
     bool ocr_b_16bit;
 } ServoMotor;
 
+// Motor Configuration
 ServoMotor motor_a = {
-    .pot_channel = 2,
+    .pot_channel = MOTOR_A_POT,
     .pin_a_port = &PORTD,
-    .pin_a_bit = PD5,
+    .pin_a_bit = MOTOR_A_PIN_A,
     .pin_a_ddr = &DDRD,
     .pin_b_port = &PORTD,
-    .pin_b_bit = PD7,
+    .pin_b_bit = MOTOR_A_PIN_B,
     .pin_b_ddr = &DDRD,
     .kp = 1.0f,
     .ki = 0.0f,
     .kd = 0.0f,
+    .target = 0,
+    .current = 0,
+    .integral = 0,
+    .last_error = 0,
     .pot_dir = 1,
     .motor_dir = 1,
     .ocr_a = &MOTOR_A_PIN_A_OCR,
     .ocr_b = &MOTOR_A_PIN_B_OCR,
+    .ocr_a_16bit = true,
+    .ocr_b_16bit = false
 };
 
 ServoMotor motor_b = {
-    .pot_channel = 3,
+    .pot_channel = MOTOR_B_POT,
     .pin_a_port = &PORTB,
-    .pin_a_bit = PB3,
+    .pin_a_bit = MOTOR_B_PIN_A,
     .pin_a_ddr = &DDRB,
     .pin_b_port = &PORTD,
-    .pin_b_bit = PD4,
+    .pin_b_bit = MOTOR_B_PIN_B,
     .pin_b_ddr = &DDRD,
     .kp = 1.0f,
     .ki = 0.0f,
     .kd = 0.0f,
+    .target = 0,
+    .current = 0,
+    .integral = 0,
+    .last_error = 0,
     .pot_dir = 1,
     .motor_dir = 1,
     .ocr_a = &MOTOR_B_PIN_A_OCR,
     .ocr_b = &MOTOR_B_PIN_B_OCR,
+    .ocr_a_16bit = false,
+    .ocr_b_16bit = true
 };
 
-void set_ocr(volatile void *reg, bool is_16bit, uint16_t value)
-{
-    if (is_16bit)
-    {
+// Function to set OCR registers (8-bit or 16-bit)
+void set_ocr(volatile void *reg, bool is_16bit, uint16_t value) {
+    if (is_16bit) {
         *((volatile uint16_t *)reg) = value;
-    }
-    else
-    {
+    } else {
         *((volatile uint8_t *)reg) = (uint8_t)value;
     }
 }
 
-void setup_pwm_motor_a(void)
-{
+// Setup Timer1 for PWM (used by both motors)
+void setup_pwm_motor_a(void) {
     // Setup Timer1 (shared)
-    DDRD |= (1 << PD5); // OC1A
-    TCCR1A |= (1 << COM1A1);
-    TCCR1A |= (1 << COM1B1);                             // Also needed for Motor B on OC1B (PD4)
+    DDRD |= (1 << PD5); // OC1A output
+    TCCR1A |= (1 << COM1A1); // Non-inverting PWM
+    TCCR1A |= (1 << COM1B1); // Also needed for Motor B on OC1B (PD4)
     TCCR1B |= (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Fast PWM, prescaler 8
-    TCCR1A |= (1 << WGM11);                              // Complete mode 14
-    ICR1 = 255;
+    TCCR1A |= (1 << WGM11); // Complete mode 14
+    ICR1 = 255; // Top value for PWM (8-bit resolution)
 
     // Setup Timer2 (OC2 for PD7)
     DDRD |= (1 << PD7);
-    TCCR2 |= (1 << WGM20) | (1 << WGM21);
+    TCCR2 |= (1 << WGM20) | (1 << WGM21); // Fast PWM
     TCCR2 |= (1 << COM21); // Non-inverting
-    TCCR2 |= (1 << CS21);
+    TCCR2 |= (1 << CS21); // Prescaler 8
 }
 
-void setup_pwm_motor_b(void)
-{
+// Setup Timer0 for PWM
+void setup_pwm_motor_b(void) {
     // Setup Timer0 (OC0 for PB3)
     DDRB |= (1 << PB3);
     TCCR0 |= (1 << WGM00) | (1 << WGM01); // Fast PWM
-    TCCR0 |= (1 << COM01);                // Non-inverting
-    TCCR0 |= (1 << CS01);
+    TCCR0 |= (1 << COM01); // Non-inverting
+    TCCR0 |= (1 << CS01); // Prescaler 8
 
     // OC1B on PD4 (Timer1 already configured in setup_pwm_motor_a)
-    DDRD |= (1 << PD4); // Just make sure it's output
+    DDRD |= (1 << PD4); // Make sure it's output
 }
 
-uint16_t read_adc(uint8_t channel)
-{
-    // Select ADC channel with MUX bits, clear left-adjust (ADMUX[5] = 0)
-    ADMUX = (1 << REFS0) | (channel & 0x07);
+// Initialize ADC
+void adc_init(void) {
+    // AREF = AVcc, ADC Left Adjust Result = 0
+    ADMUX = (1 << REFS0);
+    
+    // Enable ADC, prescaler = 128 (16MHz/128 = 125kHz)
+    ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
+}
 
+// Read ADC value
+uint16_t read_adc(uint8_t channel) {
+    // Select ADC channel with safety mask
+    ADMUX = (ADMUX & 0xF8) | (channel & 0x07);
+    
     // Start single conversion
     ADCSRA |= (1 << ADSC);
-
-    // Wait for conversion to finish
-    while (ADCSRA & (1 << ADSC))
-        ;
-
+    
+    // Wait for conversion to complete
+    while (ADCSRA & (1 << ADSC));
+    
     // Return 10-bit ADC result
     return ADC;
 }
 
-void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction)
-{
+// Control motor with PWM and direction
+void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction) {
+    // Apply motor direction correction
     direction *= motor->motor_dir;
-
-    if (pwm == 0 || motor->target == 0)
-    {
-        // Coast: both LOW
+    
+    // Stop motor if PWM is 0 or target is 0
+    if (pwm == 0 || motor->target == 0) {
+        // Coast mode: both LOW
         *(motor->pin_a_port) &= ~(1 << motor->pin_a_bit);
         *(motor->pin_b_port) &= ~(1 << motor->pin_b_bit);
         set_ocr(motor->ocr_a, motor->ocr_a_16bit, 0);
         set_ocr(motor->ocr_b, motor->ocr_b_16bit, 0);
         return;
     }
-
-    if (motor->target > 0)
-    {
-        if (direction > 0)
-        {
-            // PWM on A, B LOW
-            *(motor->pin_b_port) &= ~(1 << motor->pin_b_bit); // Direction LOW
-            set_ocr(motor->ocr_a, motor->ocr_a_16bit, pwm);
-            set_ocr(motor->ocr_b, motor->ocr_b_16bit, 0);
-        }
-        else
-        {
-            // PWM on B, A LOW
-            *(motor->pin_a_port) &= ~(1 << motor->pin_a_bit); // Direction LOW
-            set_ocr(motor->ocr_a, motor->ocr_a_16bit, 0);
-            set_ocr(motor->ocr_b, motor->ocr_b_16bit, pwm);
-        }
+    
+    // Apply direction based on target sign
+    if (direction > 0) {
+        // Forward: PWM on A, B LOW
+        *(motor->pin_b_port) &= ~(1 << motor->pin_b_bit);
+        set_ocr(motor->ocr_a, motor->ocr_a_16bit, pwm);
+        set_ocr(motor->ocr_b, motor->ocr_b_16bit, 0);
+    } else {
+        // Reverse: PWM on B, A LOW
+        *(motor->pin_a_port) &= ~(1 << motor->pin_a_bit);
+        set_ocr(motor->ocr_a, motor->ocr_a_16bit, 0);
+        set_ocr(motor->ocr_b, motor->ocr_b_16bit, pwm);
     }
 }
 
-void update_motor(ServoMotor *motor)
-{
+// PID control loop for a motor
+void update_motor(ServoMotor *motor) {
+    // Read current position from potentiometer
     motor->current = motor->pot_dir * read_adc(motor->pot_channel);
+    
+    // Calculate error
     int16_t error = motor->target - motor->current;
-
+    
+    // Update integral term with anti-windup
     motor->integral += error;
+    if (motor->integral > 1000) motor->integral = 1000;
+    if (motor->integral < -1000) motor->integral = -1000;
+    
+    // Calculate derivative term
     int16_t derivative = error - motor->last_error;
     motor->last_error = error;
-
-    int16_t output = motor->kp * error + motor->ki * motor->integral + motor->kd * derivative;
-
+    
+    // Calculate PID output
+    float output = motor->kp * error + motor->ki * motor->integral + motor->kd * derivative;
+    
+    // Determine direction and PWM value
     int8_t direction = (output >= 0) ? 1 : -1;
-    uint8_t pwm = abs(output);
+    uint8_t pwm = abs((int16_t)output);
+    
+    // Cap PWM at 255 (8-bit)
     if (pwm > 255)
         pwm = 255;
-
+    
+    // Apply control to motor
     control_motor(motor, pwm, direction);
 }
 
-uint16_t i = 64535;
-
-uint8_t reg_pointer = 0;
-bool expecting_address = true;
-
-int main(void)
-{
-    DDRA = (1 << 7); // LED
-    PORTA = (1 << 7);
-    I2C_Slave_Init(Slave_Address);
-
-    *(motor_a.pin_a_ddr) |= (1 << motor_a.pin_a_bit); // Direction pin output
-    *(motor_a.pin_b_ddr) |= (1 << motor_a.pin_b_bit); // Direction pin output
-    *(motor_b.pin_a_ddr) |= (1 << motor_b.pin_a_bit); // Direction pin output
-    *(motor_b.pin_b_ddr) |= (1 << motor_b.pin_b_bit); // Direction pin output
-
-    setup_pwm_motor_a();
-    setup_pwm_motor_b();
-
-    while (1)
-    {
-        if (!i++)
-        {
-            PORTA ^= (1 << 7);
-            i = 64535;
-        }
-        update_motor(&motor_a);
-        update_motor(&motor_b);
-        if (TWCR & (1 << TWINT))
-        {
-            /* Wait to be addressed */
-            int8_t status = TWSR & 0xF8;                 /* Read TWI status register with masking lower three bits */
-            if (status == 0x60 || status == 0x68) /* Check weather own SLA+W received & ack returned (TWEA = 1) */
-            {
-                do
-                {
-                    int8_t byte = I2C_Slave_Receive();
-                    if (byte == -1)
-                        break;
-
-                    if (expecting_address)
-                    {
-                        reg_pointer = byte;
-                        expecting_address = false;
-                    }
-                    else
-                    {
-                        ServoMotor *currentMotor;
-                        uint8_t offset = 0;
-
-                        // Decide which motor
-                        if (reg_pointer >= REGISTER_SERVOB_POSITIONH)
-                        {
-                            offset = REGISTER_SERVOB_POSITIONH;
-                            currentMotor = &motor_b;
-                        }
-                        else
-                        {
-                            offset = 0;
-                            currentMotor = &motor_a;
-                        }
-
-                        uint8_t local_reg = reg_pointer - offset;
-
-                        switch (local_reg)
-                        {
-                        case REGISTER_SERVOA_POSITIONH:
-                            currentMotor->target &= 0x00FF;
-                            currentMotor->target |= ((uint16_t)byte << 8);
-                            break;
-                        case REGISTER_SERVOA_POSITIONL:
-                            currentMotor->target &= 0xFF00;
-                            currentMotor->target |= byte;
-                            break;
-
-                        case REGISTER_SERVOA_KPA:
-                        case REGISTER_SERVOA_KPB:
-                        case REGISTER_SERVOA_KPC:
-                        case REGISTER_SERVOA_KPD:
-                            *((uint8_t *)&currentMotor->kp + (local_reg - REGISTER_SERVOA_KPA)) = byte;
-                            break;
-
-                        case REGISTER_SERVOA_KIA:
-                        case REGISTER_SERVOA_KIB:
-                        case REGISTER_SERVOA_KIC:
-                        case REGISTER_SERVOA_KID:
-                            *((uint8_t *)&currentMotor->ki + (local_reg - REGISTER_SERVOA_KIA)) = byte;
-                            break;
-
-                        case REGISTER_SERVOA_KDA:
-                        case REGISTER_SERVOA_KDB:
-                        case REGISTER_SERVOA_KDC:
-                        case REGISTER_SERVOA_KDD:
-                            *((uint8_t *)&currentMotor->kd + (local_reg - REGISTER_SERVOA_KDA)) = byte;
-                            break;
-
-                        default:
-                            // Optional: save to general-purpose register map
-                            registers[reg_pointer] = byte;
-                            break;
-                        }
-
-                        reg_pointer++; // Optional auto-increment
-                    }
-                } while (1);
-                expecting_address = true; // Reset for next transaction
+// I2C Interrupt Service Routine
+ISR(TWI_vect) {
+    uint8_t status = TWSR & 0xF8; // Read TWI status register with masking lower three bits
+    
+    // Own SLA+W received & ACK returned
+    if (status == 0x60 || status == 0x68) {
+        TWCR |= (1 << TWINT); // Clear interrupt flag to receive next byte
+        return;
+    }
+    
+    // Data received & ACK returned in SLA+W mode
+    if (status == 0x80 || status == 0x90) {
+        uint8_t received_byte = TWDR;
+        
+        if (expecting_address) {
+            reg_pointer = received_byte;
+            expecting_address = false;
+        } else {
+            ServoMotor *currentMotor;
+            uint8_t offset = 0;
+            
+            // Determine which motor based on register address
+            if (reg_pointer >= REGISTER_SERVOB_POSITIONH) {
+                offset = REGISTER_SERVOB_POSITIONH;
+                currentMotor = &motor_b;
+            } else {
+                offset = 0;
+                currentMotor = &motor_a;
             }
-            if (status == 0xA8 || status == 0xB0) /* Check weather own SLA+R received & ack returned (TWEA = 1) */
-            {
-                // READ
-                char ret;
-                ServoMotor *currentMotor;
-                uint8_t offset = 0;
-
-                // Choose motor and adjust reg_pointer for local motor indexing
-                if (reg_pointer >= REGISTER_SERVOB_POSITIONH)
-                {
-                    offset = REGISTER_SERVOB_POSITIONH;
-                    currentMotor = &motor_b;
-                }
-                else
-                {
-                    offset = 0;
-                    currentMotor = &motor_a;
-                }
-
-                uint8_t local_reg = reg_pointer - offset;
-
-                switch (local_reg)
-                {
+            
+            uint8_t local_reg = reg_pointer - offset;
+            
+            // Process register write based on local register address
+            switch (local_reg) {
                 case REGISTER_SERVOA_POSITIONH:
-                    ret = (currentMotor->current >> 8) & 0xFF;
+                    currentMotor->target &= 0x00FF;
+                    currentMotor->target |= ((uint16_t)received_byte << 8);
                     break;
+                    
                 case REGISTER_SERVOA_POSITIONL:
-                    ret = currentMotor->current & 0xFF;
+                    currentMotor->target &= 0xFF00;
+                    currentMotor->target |= received_byte;
                     break;
-
+                    
                 case REGISTER_SERVOA_KPA:
                 case REGISTER_SERVOA_KPB:
                 case REGISTER_SERVOA_KPC:
                 case REGISTER_SERVOA_KPD:
-                    ret = *((uint8_t *)&currentMotor->kp + (local_reg - REGISTER_SERVOA_KPA));
+                    *((uint8_t *)&currentMotor->kp + (local_reg - REGISTER_SERVOA_KPA)) = received_byte;
                     break;
-
+                    
                 case REGISTER_SERVOA_KIA:
                 case REGISTER_SERVOA_KIB:
                 case REGISTER_SERVOA_KIC:
                 case REGISTER_SERVOA_KID:
-                    ret = *((uint8_t *)&currentMotor->ki + (local_reg - REGISTER_SERVOA_KIA));
+                    *((uint8_t *)&currentMotor->ki + (local_reg - REGISTER_SERVOA_KIA)) = received_byte;
                     break;
-
+                    
                 case REGISTER_SERVOA_KDA:
                 case REGISTER_SERVOA_KDB:
                 case REGISTER_SERVOA_KDC:
                 case REGISTER_SERVOA_KDD:
-                    ret = *((uint8_t *)&currentMotor->kd + (local_reg - REGISTER_SERVOA_KDA));
+                    *((uint8_t *)&currentMotor->kd + (local_reg - REGISTER_SERVOA_KDA)) = received_byte;
                     break;
-
+                    
                 default:
-                    ret = registers[reg_pointer];
+                    // Store in general register map
+                    if (reg_pointer < REGISTER_COUNT) {
+                        registers[reg_pointer] = received_byte;
+                    }
                     break;
-                }
-                I2C_Slave_Transmit(ret);
-                reg_pointer++;
-                break;
             }
-            if (status == 0x70 || status == 0x78) /* Check weather general call received & ack returned (TWEA = 1) */
-                continue;                         /* If yes then return 2 to indicate ack returned */
-            else
-                continue; /* Else continue */
+            
+            // Auto-increment register pointer
+            reg_pointer++;
         }
+        
+        TWCR |= (1 << TWINT); // Clear interrupt flag
+        return;
     }
+    
+    // STOP or REPEATED START received in slave receiver mode
+    if (status == 0xA0) {
+        expecting_address = true; // Reset for next transaction
+        TWCR |= (1 << TWINT); // Clear interrupt flag
+        return;
+    }
+    
+    // Own SLA+R received & ACK returned
+    if (status == 0xA8 || status == 0xB0) {
+        ServoMotor *currentMotor;
+        uint8_t offset = 0;
+        uint8_t data_to_send = 0;
+        
+        // Determine which motor based on register address
+        if (reg_pointer >= REGISTER_SERVOB_POSITIONH) {
+            offset = REGISTER_SERVOB_POSITIONH;
+            currentMotor = &motor_b;
+        } else {
+            offset = 0;
+            currentMotor = &motor_a;
+        }
+        
+        uint8_t local_reg = reg_pointer - offset;
+        
+        // Process register read based on local register address
+        switch (local_reg) {
+            case REGISTER_SERVOA_POSITIONH:
+                data_to_send = (currentMotor->current >> 8) & 0xFF;
+                break;
+                
+            case REGISTER_SERVOA_POSITIONL:
+                data_to_send = currentMotor->current & 0xFF;
+                break;
+                
+            case REGISTER_SERVOA_KPA:
+            case REGISTER_SERVOA_KPB:
+            case REGISTER_SERVOA_KPC:
+            case REGISTER_SERVOA_KPD:
+                data_to_send = *((uint8_t *)&currentMotor->kp + (local_reg - REGISTER_SERVOA_KPA));
+                break;
+                
+            case REGISTER_SERVOA_KIA:
+            case REGISTER_SERVOA_KIB:
+            case REGISTER_SERVOA_KIC:
+            case REGISTER_SERVOA_KID:
+                data_to_send = *((uint8_t *)&currentMotor->ki + (local_reg - REGISTER_SERVOA_KIA));
+                break;
+                
+            case REGISTER_SERVOA_KDA:
+            case REGISTER_SERVOA_KDB:
+            case REGISTER_SERVOA_KDC:
+            case REGISTER_SERVOA_KDD:
+                data_to_send = *((uint8_t *)&currentMotor->kd + (local_reg - REGISTER_SERVOA_KDA));
+                break;
+                
+            default:
+                if (reg_pointer < REGISTER_COUNT) {
+                    data_to_send = registers[reg_pointer];
+                }
+                break;
+        }
+        
+        // Send data
+        TWDR = data_to_send;
+        
+        // Auto-increment register pointer
+        reg_pointer++;
+        
+        // Send ACK if not the last byte
+        if (reg_pointer < REGISTER_COUNT) {
+            TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWEA) | (1 << TWIE);
+        } else {
+            // Send NACK for last byte
+            TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWIE);
+        }
+        return;
+    }
+    
+    // Data byte transmitted & ACK received
+    if (status == 0xB8) {
+        ServoMotor *currentMotor;
+        uint8_t offset = 0;
+        uint8_t data_to_send = 0;
+        
+        // Determine which motor based on register address
+        if (reg_pointer >= REGISTER_SERVOB_POSITIONH) {
+            offset = REGISTER_SERVOB_POSITIONH;
+            currentMotor = &motor_b;
+        } else {
+            offset = 0;
+            currentMotor = &motor_a;
+        }
+        
+        uint8_t local_reg = reg_pointer - offset;
+        
+        // Process next register read
+        switch (local_reg) {
+            case REGISTER_SERVOA_POSITIONH:
+                data_to_send = (currentMotor->current >> 8) & 0xFF;
+                break;
+                
+            case REGISTER_SERVOA_POSITIONL:
+                data_to_send = currentMotor->current & 0xFF;
+                break;
+                
+            case REGISTER_SERVOA_KPA:
+            case REGISTER_SERVOA_KPB:
+            case REGISTER_SERVOA_KPC:
+            case REGISTER_SERVOA_KPD:
+                data_to_send = *((uint8_t *)&currentMotor->kp + (local_reg - REGISTER_SERVOA_KPA));
+                break;
+                
+            case REGISTER_SERVOA_KIA:
+            case REGISTER_SERVOA_KIB:
+            case REGISTER_SERVOA_KIC:
+            case REGISTER_SERVOA_KID:
+                data_to_send = *((uint8_t *)&currentMotor->ki + (local_reg - REGISTER_SERVOA_KIA));
+                break;
+                
+            case REGISTER_SERVOA_KDA:
+            case REGISTER_SERVOA_KDB:
+            case REGISTER_SERVOA_KDC:
+            case REGISTER_SERVOA_KDD:
+                data_to_send = *((uint8_t *)&currentMotor->kd + (local_reg - REGISTER_SERVOA_KDA));
+                break;
+                
+            default:
+                if (reg_pointer < REGISTER_COUNT) {
+                    data_to_send = registers[reg_pointer];
+                }
+                break;
+        }
+        
+        // Send data
+        TWDR = data_to_send;
+        
+        // Auto-increment register pointer
+        reg_pointer++;
+        
+        // Send ACK if not the last byte
+        if (reg_pointer < REGISTER_COUNT) {
+            TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWEA) | (1 << TWIE);
+        } else {
+            // Send NACK for last byte
+            TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWIE);
+        }
+        return;
+    }
+    
+    // Data byte transmitted & NACK received or last byte transmitted & ACK received
+    if (status == 0xC0 || status == 0xC8) {
+        expecting_address = true;
+        TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWEA) | (1 << TWIE);
+        return;
+    }
+    
+    // Default: re-enable TWI interrupt
+    TWCR |= (1 << TWINT);
 }
+
+// Heartbeat counter for LED blinking
+volatile uint16_t heartbeat_counter = 0;
+
+// Main function
+int main(void) {
+    // Set LED pin as output (PA7)
+    DDRA |= (1 << PA7);
+    PORTA |= (1 << PA7); // Turn on LED initially
+    
+    // Initialize I2C as slave
+    I2C_Slave_Init(Slave_Address);
+    
+    // Initialize ADC
+    adc_init();
+    
+    // Configure motor pins as outputs
+    *(motor_a.pin_a_ddr) |= (1 << motor_a.pin_a_bit);
+    *(motor_a.pin_b_ddr) |= (1 << motor_a.pin_b_bit);
+    *(motor_b.pin_a_ddr) |= (1 << motor_b.pin_a_bit);
+    *(motor_b.pin_b_ddr) |= (1 << motor_b.pin_b_bit);
+    
+    // Configure PWM for both motors
+    setup_pwm_motor_a();
+    setup_pwm_motor_b();
+    
+    // Initialize all registers to 0
+    for (uint8_t i = 0; i < REGISTER_COUNT; i++) {
+        registers[i] = 0;
+    }
+    
+    // Enable global interrupts
+    sei();
+    
+    // Main loop
+    while (1) {
+        // Update motor control
+        update_motor(&motor_a);
+        update_motor(&motor_b);
+        
+        // Heartbeat LED - toggle every ~0.5 seconds
+        heartbeat_counter++;
+        if (heartbeat_counter >= 10000) {
+            PORTA ^= (1 << PA7);
+            heartbeat_counter = 0;
+        }
+        
+        // Small delay for stability
+        _delay_us(50);
+    }
+    
+    return 0; // Never reached
+}
\ No newline at end of file