CanSat/EmbeddedAVR
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test

Browse Source
This commit is contained in:
Bruno Rybársky
2025-05-05 09:02:25 +02:00
parent 9bb4d59e11
commit df55dcfe4c
4 changed files with 452 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitingnore
View File

@@ -3,3 +3,5 @@ cmake-build-release-avr/*
cmake-build-*
build/
build
/build
/build/

132
i2c.c
View File

@@ -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 */
}

12
i2c.h
View File

@@ -2,16 +2,14 @@
* 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_ */

561
main.c
View File

@@ -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,121 +68,136 @@ 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);
@@ -190,214 +205,342 @@ void control_motor(ServoMotor *motor, uint8_t pwm, int8_t direction)
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;
// I2C Interrupt Service Routine
ISR(TWI_vect) {
uint8_t status = TWSR & 0xF8; // Read TWI status register with masking lower three bits
uint8_t reg_pointer = 0;
bool expecting_address = true;
// Own SLA+W received & ACK returned
if (status == 0x60 || status == 0x68) {
TWCR |= (1 << TWINT); // Clear interrupt flag to receive next byte
return;
}
int main(void)
{
DDRA = (1 << 7); // LED
PORTA = (1 << 7);
I2C_Slave_Init(Slave_Address);
// Data received & ACK returned in SLA+W mode
if (status == 0x80 || status == 0x90) {
uint8_t received_byte = TWDR;
*(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
if (expecting_address) {
reg_pointer = received_byte;
expecting_address = false;
} else {
ServoMotor *currentMotor;
uint8_t offset = 0;
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
// 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;
uint8_t local_reg = reg_pointer - offset;
switch (local_reg)
{
// 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
}