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

preai

Browse Source
This commit is contained in:
Bruno Rybársky
2025-05-04 20:29:47 +02:00
parent 16925fba79
commit 9bb4d59e11
1 changed files with 131 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

262
main.c
View File

@@ -49,7 +49,7 @@
#define MOTOR_B_PIN_A_OCR OCR0 #define MOTOR_B_PIN_A_OCR OCR0
#define MOTOR_B_PIN_B_OCR OCR1B #define MOTOR_B_PIN_B_OCR OCR1B
#define Slave_Address 0x20 #define Slave_Address 0x69
// I2C Slave Register Map // I2C Slave Register Map
#define REGISTER_COUNT 27 #define REGISTER_COUNT 27
@@ -228,16 +228,16 @@ void update_motor(ServoMotor *motor)
control_motor(motor, pwm, direction); control_motor(motor, pwm, direction);
} }
uint16_t i = 127; uint16_t i = 64535;
uint8_t reg_pointer = 0; uint8_t reg_pointer = 0;
bool expecting_address = true; bool expecting_address = true;
int main(void) int main(void)
{ {
I2C_Slave_Init(Slave_Address);
DDRA = (1 << 7); // LED DDRA = (1 << 7); // LED
PORTA = (1 << 7); 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_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_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_a();
setup_pwm_motor_b(); setup_pwm_motor_b();
while (1) while (1)
{ {
if (!i++) if (!i++)
{ {
PORTA ^= (1 << 7); PORTA ^= (1 << 7);
i = 60000; i = 64535;
} }
update_motor(&motor_a); update_motor(&motor_a);
update_motor(&motor_b); update_motor(&motor_b);
switch (I2C_Slave_Listen()) if (TWCR & (1 << TWINT))
{ {
case 0: /* Wait to be addressed */
{ int8_t status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
// WRITE if (status == 0x60 || status == 0x68) /* Check weather own SLA+W received & ack returned (TWEA = 1) */
do
{ {
int8_t byte = I2C_Slave_Receive(); do
if (byte == -1)
break;
if (expecting_address)
{ {
reg_pointer = byte; int8_t byte = I2C_Slave_Receive();
expecting_address = false; if (byte == -1)
} break;
else
{
ServoMotor *currentMotor;
uint8_t offset = 0;
// Decide which motor if (expecting_address)
if (reg_pointer >= REGISTER_SERVOB_POSITIONH)
{ {
offset = REGISTER_SERVOB_POSITIONH; reg_pointer = byte;
currentMotor = &motor_b; expecting_address = false;
} }
else else
{ {
offset = 0; ServoMotor *currentMotor;
currentMotor = &motor_a; 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
}
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; // Choose motor and adjust reg_pointer for local motor indexing
if (reg_pointer >= REGISTER_SERVOB_POSITIONH)
switch (local_reg) {
{ offset = REGISTER_SERVOB_POSITIONH;
case REGISTER_SERVOA_POSITIONH: currentMotor = &motor_b;
currentMotor->target &= 0x00FF; }
currentMotor->target |= ((uint16_t)byte << 8); else
break; {
case REGISTER_SERVOA_POSITIONL: offset = 0;
currentMotor->target &= 0xFF00; currentMotor = &motor_a;
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; uint8_t local_reg = reg_pointer - offset;
}
case 1:
{
// READ
char ret;
ServoMotor *currentMotor;
uint8_t offset = 0;
// Choose motor and adjust reg_pointer for local motor indexing switch (local_reg)
if (reg_pointer >= REGISTER_SERVOB_POSITIONH) {
{ case REGISTER_SERVOA_POSITIONH:
offset = REGISTER_SERVOB_POSITIONH; ret = (currentMotor->current >> 8) & 0xFF;
currentMotor = &motor_b; 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;
} }
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 else
{ continue; /* Else continue */
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;
} }
} }
} }