Updating The Embedded Code In A Balancing Robot

I took one of my past Arduino projects and updated the code to work directly with the underlying architecture on an ATmega328P. The project was originally controlled by code that utilized various Arduino libraries. I rewrote the code to use avr-libc, a C library for the AVR family of microcontrollers.

This project is described in a past post.

This code can be found on GitHub. https://github.com/PyCee/BalanceBot/blob/master/avr_balance_bot/main.c

Porting Over The Code

The first step in porting the code was to control the motors. I had 4 bits represent the state of a stepper motor’s 4 electromagnets, and, in the Arduino code, I would loop over each motor pin to set the output based on the corresponding bit.

// Define pins for the first motor
uint8_t stepperOnePins[] = {8, 9, 10, 11};
// Defien pins for the second motor
uint8_t stepperTwoPins[] = {2, 3, 4, 5};

for(int i = 0; i < 4; i++){
    // Update the output pins based on the 4 used bits in the state vars
    digitalWrite(stepperOnePins[i], stepperOneOut & (1 << i));
    digitalWrite(stepperTwoPins[i], stepperTwoOut & (1 << i));
}

With the updated code, I directly mapped the 4 bits onto the appropriate PORT registers using bitwise operators.

// Map the stepper state to ports PD2, PD3, PD4, and PD5
PORTD &= ~0b00111100;
PORTD |= (step_val << 2);
// Map the stepper state to ports PB0, PB1, PB2, and PB3
PORTB &= ~0b00001111;
PORTB |= (step_val);

The code that was the most difficult to port over was the IMU-6050 sensor communications. I had used the Wire library bundled with the Arduino software to communicate with the sensor over an I2C bus.

const uint8_t MPU = 0b1101000;
int16_t Accel[3];

Wire.beginTransmission(MPU); 
Wire.write(0x3B); 
Wire.endTransmission();
Wire.requestFrom(MPU,(uint8_t)6); 
while(Wire.available() < 6);
Accel[0] = Wire.read()<<8|Wire.read(); 
Accel[1] = Wire.read()<<8|Wire.read(); 
Accel[2] = Wire.read()<<8|Wire.read();

I used the avr-libc Two-Wire Interface (TWI) for the updated code, which is functionally the same as I2C. The below code uses the TWI to achieve the same functionality as the above Arduino code.

void start_twi(){
	TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWEA);
	while(!(TWCR & (1<<TWINT)));
	if (get_twi_status() != TW_START) exit(1);
}
void stop_twi(){
	TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN);
	while((TWCR & (1<<TWSTO)));
}
void write_twi(uint8_t data, uint8_t success_status){
	TWDR = data;
	TWCR = (1<<TWINT) | (1<<TWEN);
	while(!(TWCR & (1<<TWINT)));
	if(get_twi_status() != success_status) exit(2);
}
uint8_t read_twi(uint8_t success_status){
	TWCR = (1<<TWINT)|(1<<TWEN)|
		(((success_status == TW_MR_DATA_ACK) && 1)<<TWEA);
	while(!(TWCR & (1<<TWINT)));
	if(get_twi_status() != success_status) exit(3);
	return TWDR;
}
uint8_t get_twi_status(){
	uint8_t status;
	status = TWSR & 0xF8;
	return status;
}
void get_mpu_data(uint8_t sensor_addr, int8_t *storage){
	start_twi();
	write_twi(MPU_ADDR << 1 | TW_WRITE, TW_MT_SLA_ACK);
	write_twi(sensor_addr, TW_MT_DATA_ACK); // Sensor start data
	stop_twi();
	start_twi();
	write_twi(MPU_ADDR << 1 | TW_READ, TW_MR_SLA_ACK);
	
	// Get 16-bit sensor data along the X, Y, and Z axes
	storage[1] = read_twi(TW_MR_DATA_ACK);
	storage[0] = read_twi(TW_MR_DATA_ACK);
	storage[3] = read_twi(TW_MR_DATA_ACK);
	storage[2] = read_twi(TW_MR_DATA_ACK);
	storage[5] = read_twi(TW_MR_DATA_ACK);
	storage[4] = read_twi(TW_MR_DATA_NACK);
	stop_twi();
}

// Addresses taken from datasheet
#define MPU_ADDR 0x68
#define MPU_ACCEL_ADDR 0x3B
#define MPU_GYRO_ADDR 0x43
int16_t accel[3];
int16_t gyro[3];
get_mpu_data(MPU_ACCEL_ADDR, (int8_t*) accel);
get_mpu_data(MPU_GYRO_ADDR, (int8_t*) gyro);

Programming The Microcontroller

I used the AtmelStudio IDE with the avr-gcc compiler to compile the C code to hex, then used the avrdude command-line program to upload the file to the ATmega328P inside an Arduino Nano.

avrdude.exe -C"avrdude.conf" -v -patmega328p -carduino -PCOM4 -b115200 -D 
-Uflash:w:"$(ProjectDir)Debug\$(TargetName).hex":i