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

作者/攝影  袁佑緣
時間
  • 3小時
成本
難度   * * * * * *
材料表
  • 厚度3mm壓克力板
  • Arduino101控制板
  • 減速馬達x2
  • 麵包板
  • 杜邦線
  • 排針
  • 電池盒
  • 防滑墊
  • 3.7v鋰電池(14500)
  • 鋰電池充電器
  • TA7279P馬達驅動晶片

1. 平衡車車體製作

本範例的平衡車車體主要是由雷切壓克力構成,壓克力的厚度是使用3mm的板材,再藉由螺絲螺帽組合而成,車子的馬達使用的是兩顆小型的5V減速馬達,輪子的部份也是由壓克力組成,胎皮則是貼上一層防滑墊,車子的底部則有兩個3號電池座,裡面各裝兩顆7V的鋰電池,一個用來驅動控制板,另一個則用來驅動馬達。
詳細的CAD檔可以參考筆者放在網路上的onsahpe專案,有興趣的讀者可以參考看看喔!

圖一、平衡車的模擬動畫

圖二、組裝爆炸圖

圖四六七八、車體的三視圖

圖八、平衡車實體圖

 

2.電路接線

本次範例使用的馬達驅動晶片是TA7279P,其腳位功能與編號如下圖(節錄自官方datasheet) ,請將5號及10號腳位接到鋰電池的電源上,並把A、B兩個channel的馬達訊號輸入腳位,也就是1、3、12、13接到Arduino 101上的3、5、6、9號腳位來當作控制板的訊號控制腳位,因為這些腳位才有辦法做PWM訊號的輸出,也因此才能控制馬達的轉速。
至於馬達的部份,請將下表output(輸出)的腳位各自接到兩顆馬達上即可,例如:A channel想控制左馬達,B channel想控制右馬達,那就把4、6號腳位接到左馬達上,9、11號腳位接到右馬達上。
另外也別忘記要給TA7279馬達驅動晶片供給工作電壓,只要在14號腳位供給5V的電力,7號腳位接地就行囉。

圖九、TA7279P腳位功能圖

圖十、TA7279P腳位編號圖

圖十一、完成車體與電線配置!

3. Arduino環境準備

請先準備好Arduino IDE,並請在Arduion IDE上的Boards Manager那邊搜尋關鍵字intel,並如下圖安裝Intel Cuire Board。至於驅動程式的部份,使用MacOS跟Linux的朋友不用擔心,直接使用就可以了,那如果是windows的使用者在第一次將控制板接到電腦上時,會挑出自動安裝驅動程式的視窗,請耐心等候電腦完成安裝就可以進行到下一步囉!

圖十二、Arduino IDE Boards Manager設定

4.程式設計

#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();
	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;
}

 

5.實際操作

 

相關文章:

Leave a comment

Your email address will not be published.


*