arduino 的研究筆記

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;

}

先看mpu_6050

這兩個參數很重要
MPU6050_GYRO_FS_2000

MPU6050_ACCEL_FS_2

#define MPU6050_GYRO_FS_250 0x00
#define MPU6050_GYRO_FS_500 0x01
#define MPU6050_GYRO_FS_1000 0x02
#define MPU6050_GYRO_FS_2000 0x03

void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //扢离奀笘
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//邲蹟痀郔湮講最 +-1000僅藩鏃
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //樓厒僅僅郔湮講最 +-2G
MPU6050_setSleepEnabled(0); //輛馱釬袨怓
MPU6050_setI2CMasterModeEnabled(0); //祥MPU6050 諷秶AUXI2C
MPU6050_setI2CBypassEnabled(0); //翋諷秶腔I2C迵 MPU6050腔AUXI2C 眻籵﹝諷秶褫眕眻諉溼恀HMC5883L
}

在mpu6050 的library

initilize() 裡面

void MPU6050::initialize() {
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}

所以解析度不一樣...這裡我們修改一下

setFullScaleGyroRange()

修正了解析度

接下來看如何抓到解度....

平衡車的程式碼是呼叫 Get_Angle

void Get_Angle(u8 way)
{
float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
Temperature=Read_Temperature(); //===黍MPU6050囀离恲僅換覜杅擂ㄛ輪侔桶尨翋啣恲僅﹝
if(way==1) //===DMP腔黍婓杅擂粒摩笢剿枑倳腔奀緊ㄛ旆跡郩悜奀唗猁
{
Read_DMP(); //===黍樓厒僅﹜褒厒僅﹜褒
Angle_Balance=Pitch; //===載陔算褒
Gyro_Balance=gyro[1]; //===載陔算褒厒僅
Gyro_Turn=gyro[2]; //===載陔蛌砃褒厒僅
Acceleration_Z=accel[2]; //===載陔Z粣樓厒僅數
}
else
{
Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //黍Y粣邲蹟痀
Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //黍Z粣邲蹟痀
Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //黍X粣樓厒僅數
Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //黍Z粣樓厒僅數
if(Gyro_Y>32768) Gyro_Y-=65536; //杅擂濬倰蛌遙珩褫籵徹short秶濬倰蛌遙
if(Gyro_Z>32768) Gyro_Z-=65536; //杅擂濬倰蛌遙
if(Accel_X>32768) Accel_X-=65536; //杅擂濬倰蛌遙
if(Accel_Z>32768) Accel_Z-=65536; //杅擂濬倰蛌遙
Gyro_Balance=-Gyro_Y; //載陔算褒厒僅
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //數呾褒
Gyro_Y=Gyro_Y/16.4; //邲蹟痀講最蛌遙
if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//縐嫌霤薦疏
else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //誑硃薦疏
Angle_Balance=angle; //載陔算褒
Gyro_Turn=Gyro_Z; //載陔蛌砃褒厒僅
Acceleration_Z=Accel_Z; //===載陔Z粣樓厒僅數
}
}

============================================================

仔細看一下ameba 的版子上的IO....

仔細看.... 發覺GPIO4 和 GPIO5 可以拿來當 PWM0 和 PWM1.....

==========================================================

小车自带电机可以工作在7~13V 之间，我们使用的12V 电池供电

13 线的磁（霍尔）编码器，电机减速比1：20，

故车轮转一圈，电机可以输出260 个脉冲 (13*20 = 260)

编码器的额定电压是5V。编码器的VCC
和GND 千万不能接反，否则可能导致编码器永久损坏。

电机配有金属减速箱，12V 电压下，减速后空载转速700RPM（转每分
钟）。7.4V 电压下，减速后空载转速500RPM（转每分钟）。

充电的时候，刚开始的快速充电部分，最后是涓流充电部分，但是涓流充电
部分需要很多时间，所以充电器有两个绿灯亮（差不多半个小时多）的时候就可
以拆下电池继续使用了，等3 个灯都绿需要很长时间，建议同学不必等到3 个灯
都绿，这样需要很多时间守在充电器前面。

主板提供了常用模块接口，大家可以自行拓展。可以使用STLINK 和JLINK
调试。如果您购买了STLINK 下载调试器，可以自行连接预留的SWD 接口。只需
要连接GND IO 和SCK，安装好STLINK 驱动后就可以下载调试程序了。

利用引出来的TIM3 的引脚PA0 PA1 PA2 PA3, 接电调, 提供200HZ 的
PWM(注意是脉宽控制)控制电调,

TIM2：初始化为正交编码器模式，硬件采集编码器1 数据

TIM4：初始化为正交编码器模式，硬件采集编码器2 数据

source code 如下:

看得出來是讀硬體的計數器....然後讀走後就把他清成zero....硬體會自動累加

int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT;
TIM2 -> CNT=0;
break;

case 4: Encoder_TIM= (short)TIM4 -> CNT;
TIM4 -> CNT=0;
break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}

常規的方式只測量A相或是B相的Rising edge 或是 falling edge...

但是如果全部都考慮進去...就可以得到四倍的解析度 (A rising ...B falling... A falling... B rising)

stm32 可以直接使用硬件計數, 如果是51 系列...可以利用中斷函式...來累加

在ameba 上面 .... 不是每隻腳都可以接收interrupt...

試過例子來看... 只有 pin 3 和 pin 12 可以 (pin 2 和 pin 10 和 pin 11 不行)

看起來先不要4倍頻... 各用一隻腳.... (pin 3 和 pin 12)

check 了一下spec.... 可以發覺 GPIOA_0 有GPIO_INT 等等....

用這各表就可以找到有支援GPIO的

看有 int 的label

pin 16 17

pin 12 13

=====================================================================

ameba 也有 SoftwareSerialExample

首先先打開SoftwareSerialExamle範例, 它在 “File” -> “Examples” -> “AmebaSoftwareSerial” -> “SoftwareSerialExample”:

SoftwareSerial mySerial(0, 1); // RX, TX

和對方的 (TX ,RX) 接起來即可

設定baudrate.....
mySerial.begin(4800);

=============================================================

porting 的進度

pin layout

for encode....

// encoder0 -> motor 0
int encoder0_A = 12;
int encoder0_B = 13;

// encoder1 -> motor 1
int encoder1_A = 16;
int encoder1_B = 17;

for motor driving

// motor 0
int AIN2=4;
int AIN1=5;

// motor 1
int BIN2=6;
int BIN1=7;

// motor 0
int MOTOR_ENA=8;

// motor 1
int MOTOR_ENB=9;

===============================================================

08/17 的進度...

1. ameba 的外部供電...可由5V 的 pin 腳位給電....

2. 目前的code....似乎balance 的 KD太大...可能是高度比較高...一下子就飽和

目前在重調PD .....

做實驗

setFullScaleGyroRange(MPU6050_GYRO_FS_250);

量測數據

x_acc: -3512 y_acc: 14668 z_acc: 14668 x_gyro: 156 y_gyro: 922 z_gyro: -2
x_acc: -6632 y_acc: 13492 z_acc: 13492 x_gyro: 52 y_gyro: 1250 z_gyro: 417
x_acc: -9824 y_acc: 11900 z_acc: 11900 x_gyro: 26 y_gyro: 1226 z_gyro: 209
x_acc: -12636 y_acc: 9620 z_acc: 9620 x_gyro: -341 y_gyro: 1176 z_gyro: 545
x_acc: -12324 y_acc: 6976 z_acc: 6976 x_gyro: 1413 y_gyro: 1347 z_gyro: -774
x_acc: -15192 y_acc: 5104 z_acc: 5104 x_gyro: -30 y_gyro: 861 z_gyro: 107
x_acc: -15660 y_acc: 3248 z_acc: 3248 x_gyro: 194 y_gyro: -97 z_gyro: -857
x_acc: -14960 y_acc: 4332 z_acc: 4332 x_gyro: -340 y_gyro: 0 z_gyro: 142
x_acc: -15188 y_acc: 4456 z_acc: 4456 x_gyro: -83 y_gyro: 10 z_gyro: 116
x_acc: -15340 y_acc: 4296 z_acc: 4296 x_gyro: -139 y_gyro: -1 z_gyro: 73
x_acc: -15340 y_acc: 4324 z_acc: 4324 x_gyro: -112 y_gyro: 4 z_gyro: 128
x_acc: -15320 y_acc: 4320 z_acc: 4320 x_gyro: -116 y_gyro: 1 z_gyro: 99
x_acc: -15320 y_acc: 4276 z_acc: 4276 x_gyro: -124 y_gyro: 2 z_gyro: 91
x_acc: -15416 y_acc: 4456 z_acc: 4456 x_gyro: -122 y_gyro: 2 z_gyro: 147
x_acc: -15324 y_acc: 4388 z_acc: 4388 x_gyro: -135 y_gyro: 1 z_gyro: 114

當 setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

x_acc: -375 y_acc: 1887 z_acc: 1887 x_gyro: -339 y_gyro: 851 z_gyro: 1761
x_acc: -636 y_acc: 1809 z_acc: 1809 x_gyro: 111 y_gyro: 895 z_gyro: -21
x_acc: -911 y_acc: 1702 z_acc: 1702 x_gyro: -24 y_gyro: 789 z_gyro: 109
x_acc: -1183 y_acc: 1533 z_acc: 1533 x_gyro: 51 y_gyro: 872 z_gyro: 126
x_acc: -1328 y_acc: 1335 z_acc: 1335 x_gyro: 1223 y_gyro: 959 z_gyro: -1137
x_acc: -1649 y_acc: 1108 z_acc: 1108 x_gyro: 4 y_gyro: 859 z_gyro: 137
x_acc: -1663 y_acc: 997 z_acc: 997 x_gyro: 50 y_gyro: 714 z_gyro: 51
x_acc: -1693 y_acc: 799 z_acc: 799 x_gyro: 49 y_gyro: 790 z_gyro: -7
x_acc: -3651 y_acc: 666 z_acc: 666 x_gyro: -883 y_gyro: 666 z_gyro: 2702
x_acc: -2009 y_acc: 518 z_acc: 518 x_gyro: -432 y_gyro: -325 z_gyro: -2557
x_acc: -1875 y_acc: 506 z_acc: 506 x_gyro: -277 y_gyro: -6 z_gyro: -127
x_acc: -1897 y_acc: 556 z_acc: 556 x_gyro: -137 y_gyro: 7 z_gyro: 138

mpu6050 的 gyro 是16 bit ADC value...

-> -32768 到 32767 之間

MPU6050_GYRO_FS_250 -> 每秒 250度

MPU6050_GYRO_FS_2000 -> 每秒2000度

比方來說 MPU6050_GYRO_FS_250

-32728 ~ 32768 -> -250 ~ 250

則LSB 為 131

==============================================================

先定義朝天線端的方向為正轉,

編碼器的相位判斷

當A線為rising... 此時如果B為高電位..... 則正轉

rising... 此時如果B為低電位..... 則反轉

arduino 的研究筆記

2016年8月30日星期二

雷大的平衡車6 的研究

2016年8月29日星期一

ameba non-blocking tcp socket

ameba softserial limit , 使用D0, D1為uart時 D2(CTS)與D6(RTS)會被切成uart function

2016年8月28日星期日

雷大的 arduino github

2016年8月15日星期一

平衡小的研究筆記2

2016年8月30日 星期二