CAVEDU EducationDFRobotDFRobot Robotics
Published © GPL3+

Small Segway with Arduino 101

This project demo how to build a self-balancing robot with Arduino 101.

AdvancedFull instructions provided5 hours12,103
Small Segway with Arduino 101

Things used in this project

Hardware components

Arduino 101
Arduino 101
×1
3mm acrylic board for laser cut frame
×1
DC motor (generic)
×2
Solderless Breadboard Half Size
Solderless Breadboard Half Size
×1
Jumper wires (generic)
Jumper wires (generic)
×10
TA7279P motor driver IC
×1
3.7V lithium rechargeable battery
×2

Story

Read more

Schematics

7_RCfl4qRNyJ.jpg

4_flU1rFoyTe.jpg

5_AIvyDcZSi7.jpg

Untitled file

File missing, please reupload.

Code

Arduino 101 mini segway

Arduino
#include "CurieIMU.h" //include Arduino 101 IMU libray to detect the angular velocity and acceleration

//delare factors of PID control, note that each factor depend on the different robot and power level
const float kp = 24; 
const float ki = 0.05;
const float kd = 15;

//factor of complementary filter
const float K = 0.95;

//list number is the number of samples, the bigger it is, the more precise the angle is, but may take longer time for controller to compute it.

const int angle_list_number = 5;
const int error_list_number = 10;


//initialize motor speed with 0
int speed = 0;

//motor pins
const int motor_A_1 = 3;
const int motor_A_2 = 5;
const int motor_B_1 = 6;
const int motor_B_2 = 9;

//declare some variables to run PID control
float time, time_pre, time_step;
float gyro_angle = 0;
float acce_angle = 0;
float angle_list[angle_list_number];
float pre_error = 0;
float error_list[error_list_number];
float diff_error = 0;
float offset = 0;

void setup()
{
	for(int i = 0; i < angle_list_number; i++)
		angle_list[i] = 0.0;
	for(int i = 0; i < error_list_number; i++)
		error_list[i] = 0.0;
	pinMode(motor_A_1, OUTPUT);
	pinMode(motor_A_2, OUTPUT);
	pinMode(motor_B_1, OUTPUT);
	pinMode(motor_B_2, OUTPUT);
	pinMode(13, OUTPUT);

	Serial.begin(9600);
	Serial.println("Start!!!");

//setup IMU sensor on Arduino 101
      CurieIMU.begin();
	CurieIMU.setAccelerometerRange(4);
	CurieIMU.setGyroRange(250);
	time = millis();
	for(int i = 0; i < 5; i++)
	{
		Serial.println("Ready...");
		delay(200);	
	}
	int time2 = millis();

//target this position as this desired balance point after robot startup 2 seconds later
	while((millis()-time2) < 2000)
		offset = get_angle();
	digitalWrite(13, HIGH);
}

void loop()
{
//compute the error between the current angle and target angle repeatedly and use PID algorithm to feedback control motors
	float error = get_angle();
	float feedback = PID_feedback(error);
	if(abs(error) > 70)
//if tilt too much, then it would halt and await for restart	{
		while(true)
		{
			analogWrite(motor_A_1, 0);
			digitalWrite(motor_A_2, LOW);
			analogWrite(motor_B_1, 0);
			digitalWrite(motor_B_2, LOW);
			Serial.println("Stop!!!");
		}
	}
	balance(feedback);
}


//compute the PID feedback in balance function
void balance(float feedback)
{
	speed = int(feedback);
	if(speed < 0)
	{
		analogWrite(motor_A_1, abs(speed));
		analogWrite(motor_B_1, abs(speed));
		digitalWrite(motor_A_2, LOW);
		digitalWrite(motor_B_2, LOW);
	}
	else
	{
		digitalWrite(motor_A_1, LOW);
		digitalWrite(motor_B_1, LOW);
		analogWrite(motor_A_2, abs(speed));
		analogWrite(motor_B_2, abs(speed));
	}
}


//sum the angular velocity read from IMU along time steps, and average it, and we would use complementary filter to improve the precision of the value.

float get_angle()
{
	time_pre = time;
	time = millis();
	time_step = (time - time_pre)/1000;

	float ax, ay, az;
	float gx, gy, gz;
	CurieIMU.readAccelerometerScaled(ax, ay, az);
	CurieIMU.readGyroScaled(gx, gy, gz);


//the following serial print code are used for debuggin, since additional serial output would cost computing resource, we often comment the following lines if unncessary.	     //Serial.print(ax);
	//Serial.print("\t");
	//Serial.print(ay);
	//Serial.print("\t");
	//Serial.print(az);
	//Serial.print("\t");
	//Serial.print(gx);
	//Serial.print("\t");
	//Serial.print(gy);
	//Serial.print("\t");
	//Serial.print(gz);
	//Serial.println();
	
	gyro_angle += gy*time_step;
	acce_angle = (180/3.141593) * atan(ax/az);
	for(int i = 0; i < angle_list_number-1; i++)
		angle_list[i] = angle_list[i+1];
	angle_list[angle_list_number-1] = K * acce_angle + (1-K) * gyro_angle;
	float mean_angle;
	mean_angle = 0.0;
	for(int i = 0; i < angle_list_number; i++)
		mean_angle += angle_list[i];
	mean_angle /= 5;
	mean_angle -= offset;
	return mean_angle;

}


//implementation of PID feedback
float PID_feedback(float error)
{
	for(int i = 0; i < error_list_number-1; i++)
		error_list[i] = error_list[i+1];
	error_list[error_list_number-1] = error;

	float sum_error = 0;
	for(int i = 0; i < error_list_number; i++)
		sum_error += error_list[i];
	diff_error = error - pre_error;
	pre_error = error;
	float p_term = kp * error;
	float i_term = ki * sum_error;
	float d_term = kd * diff_error;
	float feedback = p_term + i_term + d_term;
	if(feedback >= 255)
		feedback = 255;
	else if(feedback <= -255)
		feedback = -255;

//the same as mentioned before, uncomment the lines to debug if needed

	// Serial.print("P_term: ");
	// Serial.print(p_term);
	// Serial.print("\tI_term: ");
	// Serial.print(i_term);
	// Serial.print("\tD_term: ");
	// Serial.print(d_term);
	// Serial.print("\tError: ");
	// Serial.print(error);
	// Serial.print("\tFeedback: ");
	// Serial.println(feedback);
	return feedback;
}

Credits

CAVEDU Education

CAVEDU Education

11 projects • 28 followers
We provide tutorials of robotics, Arduino, Raspberry Pi and IoT topics. http://www.cavedu.com
DFRobot

DFRobot

62 projects • 143 followers
Empowering Creation for Future Innovators
DFRobot Robotics

DFRobot Robotics

7 projects • 13 followers

Comments