diff --git a/main.c b/main.c index 9d5d8ee..a16a3b7 100644 --- a/main.c +++ b/main.c @@ -49,7 +49,7 @@ #define MOTOR_B_PIN_A_OCR OCR0 #define MOTOR_B_PIN_B_OCR OCR1B -#define Slave_Address 0x20 +#define Slave_Address 0x69 // I2C Slave Register Map #define REGISTER_COUNT 27 @@ -228,16 +228,16 @@ void update_motor(ServoMotor *motor) control_motor(motor, pwm, direction); } -uint16_t i = 127; +uint16_t i = 64535; uint8_t reg_pointer = 0; bool expecting_address = true; int main(void) { - I2C_Slave_Init(Slave_Address); 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 @@ -247,157 +247,157 @@ int main(void) setup_pwm_motor_a(); setup_pwm_motor_b(); - while (1) { if (!i++) { PORTA ^= (1 << 7); - i = 60000; + i = 64535; } update_motor(&motor_a); update_motor(&motor_b); - switch (I2C_Slave_Listen()) + if (TWCR & (1 << TWINT)) { - case 0: - { - // WRITE - do + /* 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) */ { - int8_t byte = I2C_Slave_Receive(); - if (byte == -1) - break; - - if (expecting_address) + do { - reg_pointer = byte; - expecting_address = false; - } - else - { - ServoMotor *currentMotor; - uint8_t offset = 0; + int8_t byte = I2C_Slave_Receive(); + if (byte == -1) + break; - // Decide which motor - if (reg_pointer >= REGISTER_SERVOB_POSITIONH) + if (expecting_address) { - offset = REGISTER_SERVOB_POSITIONH; - currentMotor = &motor_b; + reg_pointer = byte; + expecting_address = false; } else { - offset = 0; - currentMotor = &motor_a; + 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 + } + if (status == 0xA8 || status == 0xB0) /* Check weather own SLA+R received & ack returned (TWEA = 1) */ + { + // READ + char ret; + ServoMotor *currentMotor; + uint8_t offset = 0; - 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 + // 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; } - } while (1); - expecting_address = true; // Reset for next transaction - break; - } - case 1: - { - // READ - char ret; - ServoMotor *currentMotor; - uint8_t offset = 0; + uint8_t local_reg = reg_pointer - offset; - // Choose motor and adjust reg_pointer for local motor indexing - if (reg_pointer >= REGISTER_SERVOB_POSITIONH) - { - offset = REGISTER_SERVOB_POSITIONH; - currentMotor = &motor_b; + 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; } + 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 - { - 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; + continue; /* Else continue */ } } }