[ Arduino101 專題實作計畫] Arduino101二輪平衡小車DIY

#include "CurieIMU.h" //引入Arduino 101 IMU的函式庫，用來偵測車體的            角速度與加速度方向

//宣告PID控制的各個參數，各個參數的最佳數值必須視車體情況而定，不同的車體會有所不同，電力的大小也會有影響
const float kp = 24; 
const float ki = 0.05;
const float kd = 15;

//宣告參數K作為complimentray filter的參數
const float K = 0.95;

//list number的大小會決定取平均的樣本數，越多則越準，但也會花掉更多計算時間
const int angle_list_number = 5;
const int error_list_number = 10;

//設定馬達初速為0
int speed = 0;

//定義馬達的驅動腳位
const int motor_A_1 = 3;
const int motor_A_2 = 5;
const int motor_B_1 = 6;
const int motor_B_2 = 9;

//宣告進行角度計算與PID控制會用到的一些參數
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!!!");

//設定Arduino 101 的IMU感測器	
      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();

//待機兩秒後，取得一個初始位置的角度，這個位置將會是平衡車目標的平衡位置
	while((millis()-time2) < 2000)
		offset = get_angle();
	adjust_timer = millis();
	digitalWrite(13, HIGH);
}

void loop()
{
//主迴圈會一直去讀取現在角度與目標角度的誤差，並透過PID控制來回傳修正動作給馬達
	float error = get_angle();
	float feedback = PID_feedback(error);
	if(abs(error) > 70)
//當傾斜角度過大時，會視為倒掉，此時將會停機並等待重啟
	{
		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);
}

//平衡函式將會根據PID算出的回饋數值，呼叫馬達做出相對應的修正動作
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));
	}
}

//讀取角度的函式會透過計時器累加的部份，將IMU讀到的角速度離散積分成角度，同時會做平均取值並輔以complimentary filter的方式來將精確的角度數值計算出來
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);

//將以下的註解取消掉的話可以從Arduino 101的Serial讀到此刻感應器的原始數值，記得baud rate要跟前面宣告的一樣設定成9600，另外要提醒一下如果開啟Serial輸出的功能的話，會消耗掉額外的計算資源，所以如果已經不需要再讀取數值時，最好把以下的程式碼註解掉
	//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;

}

//PID回饋的函式會將錯誤進行一連串的計算，並根據開頭我們設定的三個係數來做出適當的回饋
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;

//跟前面一樣，把以下程式碼的註解拿掉的話可以從Serial讀出實際得出的回饋
	// 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;
}