update

CMakeLists.txt

@@ -143,5 +143,7 @@ avr_generate_fixed_targets()
 
 add_avr_executable(
         avrtest
+        i2c.c
+        i2c.h
         main.c
 )

i2c.c

@@ -0,0 +1,74 @@
+/*
+ * 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 */
+}
+
+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;
+}
+
+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 */
+}

i2c.h

@@ -0,0 +1,17 @@
+/*
+ * 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 */
+
+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_ */

main.c

@@ -2,9 +2,39 @@
 #include <avr/interrupt.h>
 #include <util/delay.h>
 #include <stdlib.h>
+#include "i2c.h"
 #include <stdbool.h>
 #include <avr/iom32.h>
 
+#define REGISTER_SERVOA_POSITIONH 0
+#define REGISTER_SERVOA_POSITIONL 1
+#define REGISTER_SERVOA_KPA 2
+#define REGISTER_SERVOA_KPB 3
+#define REGISTER_SERVOA_KPC 4
+#define REGISTER_SERVOA_KPD 5
+#define REGISTER_SERVOA_KIA 6
+#define REGISTER_SERVOA_KIB 7
+#define REGISTER_SERVOA_KIC 8
+#define REGISTER_SERVOA_KID 9
+#define REGISTER_SERVOA_KDA 10
+#define REGISTER_SERVOA_KDB 11
+#define REGISTER_SERVOA_KDC 12
+#define REGISTER_SERVOA_KDD 13
+#define REGISTER_SERVOB_POSITIONH 14
+#define REGISTER_SERVOB_POSITIONL 15
+#define REGISTER_SERVOB_KPA 16
+#define REGISTER_SERVOB_KPB 17
+#define REGISTER_SERVOB_KPC 18
+#define REGISTER_SERVOB_KPD 19
+#define REGISTER_SERVOB_KIA 20
+#define REGISTER_SERVOB_KIB 21
+#define REGISTER_SERVOB_KIC 22
+#define REGISTER_SERVOB_KID 23
+#define REGISTER_SERVOB_KDA 24
+#define REGISTER_SERVOB_KDB 25
+#define REGISTER_SERVOB_KDC 26
+#define REGISTER_SERVOB_KDD 27
+
 // Motor A
 #define MOTOR_A_POT 2
 #define MOTOR_A_PIN_A PD5
@@ -19,15 +49,18 @@
 #define MOTOR_B_PIN_A_OCR OCR0
 #define MOTOR_B_PIN_B_OCR OCR1B
 
+#define Slave_Address 0x20
+
 // I2C Slave Register Map
-#define REGISTER_COUNT 16
+#define REGISTER_COUNT 27
 volatile uint8_t registers[REGISTER_COUNT];
 
 // I2C State
 volatile uint8_t reg_address = 0;
 volatile bool reg_address_received = false;
 
-typedef struct {
+typedef struct
+{
     uint8_t pot_channel;
     volatile uint8_t *pin_a_port;
     volatile uint8_t *pin_a_ddr;
@@ -54,9 +87,15 @@ typedef struct {
 
 ServoMotor motor_a = {
     .pot_channel = 2,
-        .pin_a_port = &PORTD, .pin_a_bit = PD5, .pin_a_ddr = &DDRD,
+    .pin_a_port = &PORTD,
-        .pin_b_port = &PORTD, .pin_b_bit = PD7, .pin_b_ddr = &DDRD,
+    .pin_a_bit = PD5,
-        .kp = 1.0f, .ki = 0.0f, .kd = 0.0f,
+    .pin_a_ddr = &DDRD,
+    .pin_b_port = &PORTD,
+    .pin_b_bit = PD7,
+    .pin_b_ddr = &DDRD,
+    .kp = 1.0f,
+    .ki = 0.0f,
+    .kd = 0.0f,
     .pot_dir = 1,
     .motor_dir = 1,
     .ocr_a = &MOTOR_A_PIN_A_OCR,
@@ -65,24 +104,35 @@ ServoMotor motor_a = {
 
 ServoMotor motor_b = {
     .pot_channel = 3,
-        .pin_a_port = &PORTB, .pin_a_bit = PB3, .pin_a_ddr = &DDRB,
+    .pin_a_port = &PORTB,
-        .pin_b_port = &PORTD, .pin_b_bit = PD4, .pin_b_ddr = &DDRD,
+    .pin_a_bit = PB3,
-        .kp = 1.0f, .ki = 0.0f, .kd = 0.0f,
+    .pin_a_ddr = &DDRB,
+    .pin_b_port = &PORTD,
+    .pin_b_bit = PD4,
+    .pin_b_ddr = &DDRD,
+    .kp = 1.0f,
+    .ki = 0.0f,
+    .kd = 0.0f,
     .pot_dir = 1,
     .motor_dir = 1,
     .ocr_a = &MOTOR_B_PIN_A_OCR,
     .ocr_b = &MOTOR_B_PIN_B_OCR,
 };
 
-void set_ocr(volatile void *reg, bool is_16bit, uint16_t value) {
+void set_ocr(volatile void *reg, bool is_16bit, uint16_t value)
-    if (is_16bit) {
+{
+    if (is_16bit)
+    {
         *((volatile uint16_t *)reg) = value;
-    } else {
+    }
+    else
+    {
         *((volatile uint8_t *)reg) = (uint8_t)value;
     }
 }
 
-void setup_pwm_motor_a(void) {
+void setup_pwm_motor_a(void)
+{
     // Setup Timer1 (shared)
     DDRD |= (1 << PD5); // OC1A
     TCCR1A |= (1 << COM1A1);
@@ -98,7 +148,8 @@ void setup_pwm_motor_a(void) {
     TCCR2 |= (1 << CS21);
 }
 
-void setup_pwm_motor_b(void) {
+void setup_pwm_motor_b(void)
+{
     // Setup Timer0 (OC0 for PB3)
     DDRB |= (1 << PB3);
     TCCR0 |= (1 << WGM00) | (1 << WGM01); // Fast PWM
@@ -109,29 +160,28 @@ void setup_pwm_motor_b(void) {
     DDRD |= (1 << PD4); // Just make sure it's output
 }
 
-
+uint16_t read_adc(uint8_t channel)
-uint16_t read_adc(uint8_t channel) {
+{
-    // Mask channel to stay within 0–7
-    channel &= 0x07;
-
     // Select ADC channel with MUX bits, clear left-adjust (ADMUX[5] = 0)
-    ADMUX = (ADMUX & 0xF0) | channel;
+    ADMUX = (1 << REFS0) | (channel & 0x07);
 
     // Start single conversion
     ADCSRA |= (1 << ADSC);
 
     // Wait for conversion to finish
-    while (ADCSRA & (1 << ADSC));
+    while (ADCSRA & (1 << ADSC))
+        ;
 
     // Return 10-bit ADC result
     return ADC;
 }
 
-
+void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction)
-void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction) {
+{
     direction *= motor->motor_dir;
 
-    if (pwm == 0) {
+    if (pwm == 0 || motor->target == 0)
+    {
         // Coast: both LOW
         *(motor->pin_a_port) &= ~(1 << motor->pin_a_bit);
         *(motor->pin_b_port) &= ~(1 << motor->pin_b_bit);
@@ -140,21 +190,27 @@ void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction) {
         return;
     }
 
-    if (direction > 0) {
+    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 {
+        }
+        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);
         }
     }
+}
 
-
+void update_motor(ServoMotor *motor)
-void update_motor(ServoMotor *motor) {
+{
     motor->current = motor->pot_dir * read_adc(motor->pot_channel);
     int16_t error = motor->target - motor->current;
 
@@ -166,77 +222,23 @@ void update_motor(ServoMotor *motor) {
 
     int8_t direction = (output >= 0) ? 1 : -1;
     uint8_t pwm = abs(output);
-    if (pwm > 255) pwm = 255;
+    if (pwm > 255)
+        pwm = 255;
 
     control_motor(motor, pwm, direction);
 }
 
+uint16_t i = 127;
 
-uint8_t i = 127;
+uint8_t reg_pointer = 0;
+bool expecting_address = true;
 
-ISR(TWI_vect) {
+int main(void)
-    switch (TWSR & 0xF8) {
+{
-        case 0x60: // Own SLA+W received, ACK returned
+    I2C_Slave_Init(Slave_Address);
-        case 0x68: // Arbitration lost, own SLA+W received, ACK returned
-            reg_address_received = false; // Reset for new transfer
-            TWCR |= (1 << TWINT) | (1 << TWEA); // ACK next byte
-            break;
-
-        case 0x80: // Data received, ACK returned
-        case 0x90: // Data received (General Call), ACK returned
-            if (!reg_address_received) {
-                reg_address = TWDR; // First received byte = register address
-                reg_address_received = true;
-            } else {
-                if (reg_address < REGISTER_COUNT) {
-                    registers[reg_address++] = TWDR; // Store received data, then auto-increment address
-                }
-            }
-            TWCR |= (1 << TWINT) | (1 << TWEA); // ACK next byte
-            break;
-
-        case 0xA8: // Own SLA+R received, ACK returned
-        case 0xB0: // Arbitration lost, own SLA+R received, ACK returned
-            if (reg_address < REGISTER_COUNT) {
-                TWDR = registers[reg_address++]; // Load data to send
-            } else {
-                TWDR = 0xFF; // Out of range, send dummy
-            }
-            TWCR |= (1 << TWINT) | (1 << TWEA); // ACK next byte
-            break;
-
-        case 0xB8: // Data transmitted, ACK received
-            if (reg_address < REGISTER_COUNT) {
-                TWDR = registers[reg_address++];
-            } else {
-                TWDR = 0xFF;
-            }
-            TWCR |= (1 << TWINT) | (1 << TWEA); // ACK next byte
-            break;
-
-        case 0xC0: // Data transmitted, NACK received (done)
-        case 0xC8: // Last byte transmitted, ACK received
-        case 0x88: // Data received, NACK returned
-            TWCR |= (1 << TWINT) | (1 << TWEA); // Done
-            break;
-
-        case 0x00: // Bus error
-            TWCR |= (1 << TWSTO) | (1 << TWINT) | (1 << TWEA); // Recover
-            break;
-
-        default:
-            TWCR |= (1 << TWINT) | (1 << TWEA); // Default ACK
-            break;
-    }
-}
-
-
-int main(void) {
-    ADCSRA |= (1 << ADEN);
     DDRA = (1 << 7); // LED
+    PORTA = (1 << 7);
 
-    TWAR = (0x69 << 1) | (1 << 0);
-    TWCR = (1 << 6) | (1 << 2) | (1 << 0);
     *(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
@@ -246,13 +248,156 @@ int main(void) {
     setup_pwm_motor_b();
 
 
-    while (1) {
+    while (1)
-        if (!i++) {
+    {
+        if (!i++)
+        {
             PORTA ^= (1 << 7);
-            i = 127;
+            i = 60000;
         }
         update_motor(&motor_a);
         update_motor(&motor_b);
-        _delay_ms(10);
+        switch (I2C_Slave_Listen()) 
+        {
+        case 0:
+        {
+            // WRITE
+            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
+
+            break;
+        }
+        case 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)
+            {
+            case REGISTER_SERVOA_POSITIONH:
+                ret = (currentMotor->current >> 8) & 0xFF;
+                break;
+            case REGISTER_SERVOA_POSITIONL:
+                ret = currentMotor->current & 0xFF;
+                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));
+                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));
+                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));
+                break;
+
+            default:
+                ret = registers[reg_pointer];
+                break;
+            }
+            I2C_Slave_Transmit(ret);
+            reg_pointer++;
+            break;
+        }
+        default:
+            break;
+        }
     }
 }