diff --git a/CMakeLists.txt b/CMakeLists.txt index ddb8cf9..853ab7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,5 +143,7 @@ avr_generate_fixed_targets() add_avr_executable( avrtest + i2c.c + i2c.h main.c ) \ No newline at end of file diff --git a/i2c.c b/i2c.c new file mode 100644 index 0000000..9a1188e --- /dev/null +++ b/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< /* 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_ */ \ No newline at end of file diff --git a/main.c b/main.c index 123ba2b..9d5d8ee 100644 --- a/main.c +++ b/main.c @@ -2,32 +2,65 @@ #include #include #include +#include "i2c.h" #include #include +#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 -#define MOTOR_A_PIN_B PD7 -#define MOTOR_A_PIN_A_OCR OCR1A -#define MOTOR_A_PIN_B_OCR OCR2 +#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 -#define MOTOR_B_PIN_A_OCR OCR0 -#define MOTOR_B_PIN_B_OCR OCR1B +#define MOTOR_B_POT 3 +#define MOTOR_B_PIN_A PB3 +#define MOTOR_B_PIN_B PD4 +#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; @@ -44,7 +77,7 @@ typedef struct { float integral; int16_t last_error; - bool pot_dir; // 1 or -1 + bool pot_dir; // 1 or -1 bool motor_dir; // 1 or -1 volatile void *ocr_a; volatile void *ocr_b; @@ -53,42 +86,59 @@ typedef struct { } ServoMotor; ServoMotor motor_a = { - .pot_channel = 2, - .pin_a_port = &PORTD, .pin_a_bit = PD5, .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, - .ocr_b = &MOTOR_A_PIN_B_OCR, + .pot_channel = 2, + .pin_a_port = &PORTD, + .pin_a_bit = PD5, + .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, + .ocr_b = &MOTOR_A_PIN_B_OCR, }; ServoMotor motor_b = { - .pot_channel = 3, - .pin_a_port = &PORTB, .pin_a_bit = PB3, .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, + .pot_channel = 3, + .pin_a_port = &PORTB, + .pin_a_bit = PB3, + .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) { - if (is_16bit) { - *((volatile uint16_t *) reg) = value; - } else { - *((volatile uint8_t *) reg) = (uint8_t) value; +void set_ocr(volatile void *reg, bool is_16bit, uint16_t value) +{ + if (is_16bit) + { + *((volatile uint16_t *)reg) = value; + } + 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); - TCCR1A |= (1 << COM1B1); // Also needed for Motor B on OC1B (PD4) + 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 + TCCR1A |= (1 << WGM11); // Complete mode 14 ICR1 = 255; // Setup Timer2 (OC2 for PD7) @@ -98,40 +148,40 @@ 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 - TCCR0 |= (1 << COM01); // Non-inverting + TCCR0 |= (1 << COM01); // Non-inverting TCCR0 |= (1 << CS01); // OC1B on PD4 (Timer1 already configured in setup_pwm_motor_a) DDRD |= (1 << PD4); // Just make sure it's output } - -uint16_t read_adc(uint8_t channel) { - // Mask channel to stay within 0–7 - channel &= 0x07; - +uint16_t read_adc(uint8_t channel) +{ // 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) { - // 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); + 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); + } } } - -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,93 +222,182 @@ 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) { - switch (TWSR & 0xF8) { - case 0x60: // Own SLA+W received, ACK returned - 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; +int main(void) +{ + I2C_Slave_Init(Slave_Address); + DDRA = (1 << 7); // LED + PORTA = (1 << 7); - 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 - - 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 - *(motor_b.pin_b_ddr) |= (1 << motor_b.pin_b_bit); // Direction pin output + *(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++) { + while (1) + { + 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 *)¤tMotor->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 *)¤tMotor->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 *)¤tMotor->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 *)¤tMotor->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 *)¤tMotor->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 *)¤tMotor->kd + (local_reg - REGISTER_SERVOA_KDA)); + break; + + default: + ret = registers[reg_pointer]; + break; + } + I2C_Slave_Transmit(ret); + reg_pointer++; + break; + } + default: + break; + } } }