雷大的平衡車6 的研究

Ref : https://github.com/regishsu/arduino/blob/master/balance6/balance6.ino





void Position_Calculate(void){

speed_r_l =(speed_mr + speed_ml) * 0.5;
car_speed *= 0.7; //車輪速度濾波
car_speed += speed_r_l * 0.3;
car_position += car_speed; //積分得到位移
car_position += speed_need;
if (car_position < -3500) car_position = -3500;
if (car_position > 3500)
car_position = 3500;
PWM_output = Kp * Angle + Kd * Gyro_y + Kpn * car_position + Ksp * car_speed;
PWM_L = PWM_output + turn_need;
PWM_R = PWM_output - turn_need;

if (cmd_sw_motor){

if(PWM_L > 1)//左电机-------或者取0
{
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
}

else if(PWM_L < -1)//-------或者取0
	{
	digitalWrite(TN1, LOW);
	digitalWrite(TN2, HIGH);
	}
	else //刹车-------取0后可以不用
	{
	digitalWrite(TN1, HIGH);
	digitalWrite(TN2, HIGH);
	}

if(PWM_R > 1)//右电机--------或者取0
	{
	digitalWrite(TN3, HIGH);
	digitalWrite(TN4, LOW);
	}
	else if(PWM_R < -1)//-------或者取0
	{
	digitalWrite(TN3, LOW);
	digitalWrite(TN4, HIGH);
	}
	else//刹车-------取0后可以不用
	{
	digitalWrite(TN3, HIGH);
	digitalWrite(TN4, HIGH);
	}

PWM_L = min(250, (abs(PWM_L) + cmd_DZ_L)); PWM_R = min(250, (abs(PWM_R) + cmd_DZ_R)); analogWrite(ENA, PWM_L); analogWrite(ENB, PWM_R);

}else{
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
}

// for motor encoder....
attachInterrupt(0, Motor_Count_L, RISING); // motor counter
attachInterrupt(1, Motor_Count_R, RISING); // motor counter

void Motor_Count_R(void)
	{
	if (digitalRead(TN3))
	speed_mr += 1;
	else
	speed_mr -= 1;
	}

void Motor_Count_L(void)
	{
	//blink_led = !blink_led;
	//digitalWrite(led, blink_led);
	if (digitalRead(TN1))
	speed_ml += 1;
	else
	speed_ml -= 1;
	}

static long task_dt = 0; //10000=10ms, task_dt為主循環執行時間
Timer1.initialize(task_dt); //1s
Timer1.attachInterrupt(Timer1_callback); // attaches callback() as a timer overflow interrupt

void Timer1_callback(void)
	{
	//blink_led = !blink_led;
	//digitalWrite(led, blink_led);
	timer1_sw++;
	}

void Angle_Calculate(void){

Accel_x = Gyro6050.getAccelerationY();

Gyro_y = Gyro6050.getRotationX(); Angle_ax = (Accel_x + Accel_x_offset) /16384; //去除零點偏移,計算得到角度（弧度）

Angle_ax = Angle_ax * 1.3 * RAD_TO_DEG; //弧度轉換為度,(180/3.14)

Gyro_y = -(Gyro_y - Gyro_y_offset)/131; //去除零點偏移，計算角速度值,負號為方向處理

       Kalman_Filter(Angle_ax, Gyro_y); //卡爾曼濾波計算傾角

}

void loop() {

if (timer1_sw){

Angle_Calculate();

if (abs(Angle) < 40){

Position_Calculate();

}else{

analogWrite(ENA, 0);

analogWrite(ENB, 0);

}

timer1_sw--;

speed_mr = speed_ml = 0;

}

}