本帖最后由 xin6602 于 2015-8-13 18:42 编辑
但那个测速块我应该是接的不对。先仔细看完帖子吧，有不懂的还请楼主多指教。谢谢！
楼主棒棒的，近段时间也在做自平衡小车，有问题想请教你，加你Q了没见同意570802889
新手213123 发表于 2015-6-14 09:22 static/image/common/back.gif
我现在整体结构都改了，调整范围大了。但是有个问题就是我的解算角度会偏得很严重，就是上下来回的飘，像 ...
P、D的参数没有调好
xin6602 发表于 2015-8-13 18:22 static/image/common/back.gif
非常感谢楼主的分享！在这里找到了最完整的教程。谢谢！！ 开始也是用楼主一样的小车车架，为了充分体现能站 ...
你这个很漂亮，视频先发到优酷上，然后再把链接发到这里即可
本帖最后由 lieak59 于 2016-4-4 17:03 编辑
不好意思 请问你 43楼 . 1楼 的 D2S元件 和 M5 SP元件 都各是啥元件?
还有如果用双测速模块两测速码盘光电编码器脉冲输出模组
像这种的有没有办法?接在电路里面
看了1楼的电路 你是用4脚的测速模组 可是3脚的有没有办法?
左边VCC 中间GND 右边OUTPUT
43楼 虽然少了测速模组 但还是不知道PCB裡 D2S 跟 M5 SP CB元件 的名称
楼主请问你的测速传感器是全名是?
lieak59 发表于 2016-4-2 10:40 static/image/common/back.gif
不好意思 请问你 43楼 . 1楼 的 D2S元件 和 M5 SP元件 都各是啥元件?
还有如果用双测速模块两测速码盘光 ...
D2S只是一个信号选择的跳线，意思是D2可以用于测速，或者用于GY-521的中断检测；M5/SP是测速传感器（这是一个双测速，左右测速），跟你用2个三脚的测速传感器是一回事。
本帖最后由 lieak59 于 2016-4-13 20:57 编辑
抱歉大大我有几个问题要问
#56的串口调试工具 sscom4.2
3、在程序每次输出小车实际角度的同时，输出Arduino的系统时间[用millis()函数]，下载并运行程序用串口调试助手记录系统时间与MPU6050的角度输出，记录足够的数据后“保存窗口”数据，用Excel打开记录的数据文件，观察数据稳定时间。
millis()函数在哪 我漏看啥? 程式码了我是用WJY_DIY_v1_4_2.ino 下去做 只有这个才有双测速器的程式码
再来蓝芽调适的工具我都下了 可是没发现蓝芽控制的程式码阿0.0 我漏看了啥么吗?
4、确定完这个稳定时间后，将程序输出恢复到原来状态（去掉时间输出部分），重新下载，去掉小车支架并启动运行；将小车放置于一固角度位置，保持不动，观察角度输出数据是否稳定（这个时间必须大于第3步所测时间），待数据稳定后，用手扶着小车找其自然物理平衡角；通过串口工具输出“G”或“g”将平衡设定角“setp0”设置到与小车的自然物理平衡角相一致。
恕我语言能力太差 不太懂将轿车放置于一个固定角度位子(要用墙脚卡车子吗?) 时间要比第3步的还要久?
也不太懂 通过串口工具输出“G”或“g”将平衡设定角“setp0”设置到与小车的自然物理平衡角相一致。
5、这此之后小车的整定过程中不要断电，始终保持运行状态，逐步增加Kp，观察小车的两轮是否有抵抗角度变化的力量，如果不仅不能抵抗角度的变化，而是顺着角度变化（增大角度的变化），则将顺其变化的控制电机接线反过来接，再继续下一步。
这边不懂得的是不断电意思是都要一直用外接电源了? 再来是抵抗角度变化与顺着角度变化 大概知道是为了要让它平衡才让车子反转吧
目前車子已經裝好做好微調都沒問題了 電源部分獲得了改善 再來是慢慢調車子了 可是條車子卻沒看到藍芽程式碼與你所說的函數 最近忙與混亂 想東西是不是漏看了啥 再來找你 可是資料都還在慢慢翻弄得自己特別混亂
串口都照着调了 接下来运行程式不能跟arduino序列卓视窗一起共用 是要特别去而外勾那些Hex的数值吗?
还有数据会存在哪里找了一堆档都没看到呢@.@?
有给档名 可是开寻找也没找到那档名 里面有日文
修改过的WJY_DIY_v1_4_2.ino程式码
//原始6050数据+双测速码盘
#include "Wire.h" //串口
#include "I2Cdev.h" //IIC总线
#include "MPU6050.h" //加速度与陀螺传感器
//自定义变量
char val='z'; //调节与控制命令字
int Speed_need = 0, Turn_need = 0; //运动速度，转弯速度
float speeds, speeds_filter, positions; //速度，速度滤波，位置
float diff_speeds, dspeeds = 0, dspeeds_all = 0; //速度差
int text_time = 0, spcount = 0, dspcount = 0; //测试时间，速度测量次数
int U1_Pin = 2;
int U2_Pin = 3;
float Val = 0; //设置变量Val，计数
float time;//设置变量time，计时
float Speed;//设置变量Speed，存储转速
//PID参数
double Output = 0; //PID输出
float Kp=0.0, Kd=0.00, Ksp = 0.0, Ksi = 0.00, Kdsp = 0; //PID角度环、速度环系数
// MPU6050参数
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;
//角度参数
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //一阶互补滤波计算出的小车最终倾斜角度
float Angle0 = -13.3; //机械平衡角
//引脚分配
int PinA_left = 2; //中断0
int PinA_right = 3; //中断1
int M_left = 8;
int M_left2 = 7;
int E_left = 6; //ENA
int M_right = 9;
int M_right2= 10;
int E_right = 11; //ENB
//电机输出
int PWM_right = 0, PWM_left = 0;
int PWM_left_least = 0, PWM_right_least = 0; //左右电机启动补偿50
//测速码盘中断
int count_right = 0;
int count_left = 0;
void Code_left() {
if(Output>=0) {
count_left ++;
}else {
count_left --;
}
} //左测速码盘计数
void Code_right() {
if(Output>=0) {
count_right ++;
}else {
count_right --;
}
} //右测速码盘计数
//电机输出
void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0) {
digitalWrite(M_left, LOW);
digitalWrite(M_left2, HIGH);
}else {
digitalWrite(M_left, HIGH);
digitalWrite(M_left2, LOW);
nLeftVol = -nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right, LOW);
digitalWrite(M_right2, HIGH);
}else {
digitalWrite(M_right, HIGH);
digitalWrite(M_right2, LOW);
nRightVol = -nRightVol;
}
if(nLeftVol > 255) nLeftVol = 255; //防止PWM值超过255
if(nRightVol > 255) nRightVol = 255; //防止PWM值超过255
analogWrite(E_left, nLeftVol);
analogWrite(E_right, nRightVol);
}
//计算小车角度
void Angle_Calcu(void) {
Angle_ax = (ax - 711.909)/238.2 ; //去除零点偏移，16384*3.14/1.2/180,弧度转换为度
Gyro_y = -(gy - 85.607)/16.4; //去除零点偏移，2000deg/s 16.4 LSB/(deg/s)250---131
Angle = 0.97 * (Angle + Gyro_y * 0.0005) + 0.03 * Angle_ax;
}
void setup() {
Wire.begin();
Serial.begin(115200);
accelgyro.initialize(); //初始化设备
//引脚模式设置
pinMode(E_left, OUTPUT);
pinMode(M_left, OUTPUT);
pinMode(M_left2, OUTPUT); //左电机
pinMode(E_right, OUTPUT);
pinMode(M_right, OUTPUT);
pinMode(M_right2, OUTPUT); //右电机
pinMode(PinA_right, INPUT);
pinMode(PinA_left, INPUT); //测速码盘输入
//中断设置
attachInterrupt(0, Code_right, CHANGE);
attachInterrupt(1, Code_left, CHANGE);
}
void loop() {
if (Serial.available() > 0) {
val = Serial.read();
//参数调节
if(val == 'A') Kp += 0.1;
if(val == 'a') Kp -= 0.1;
if(val == 'B') Kd += 0.01;
if(val == 'b') Kd -= 0.01;
if(val == 'C') Ksp += 0.1;
if(val == 'c') Ksp -= 0.1;
if(val == 'D') Ksi += 0.01;
if(val == 'd') Ksi -= 0.01;
if(val == 'E') Angle0 += 0.1;
if(val == 'e') Angle0 -= 0.1;
if(val == 'F') PWM_left_least += 1;
if(val == 'f') PWM_left_least -= 1;
if(val == 'G') PWM_right_least += 1;
if(val == 'g') PWM_right_least -= 1;
//参数查看
if(val == 'H') {
Serial.print(ax); Serial.print("\t"); //用于测量零点偏移
Serial.println(gy); //用于测量零点偏移
}
if(val == 'I') {
Serial.print(Angle0); Serial.print("\t");
Serial.print(PWM_left_least); Serial.print("\t");
Serial.print(PWM_right_least); Serial.print("\t");
Serial.print(Kp); Serial.print("\t");
Serial.print(Kd); Serial.print("\t");
Serial.print(Ksp); Serial.print("\t");
Serial.print(Ksi); Serial.print("\t");
Serial.println(Kdsp);
}
if(val == 'J') {
Serial.print(Angle); Serial.print("\t");
Serial.println(dspeeds_all);
}
if(val == 'K') Kdsp += 1;
if(val == 'k') Kdsp -= 1;
//小车控制
if(val == '1') {
Speed_need = 15; //前进
Turn_need = 0;
}
if(val == '2') {
Speed_need = -15; //后退
Turn_need = 0;
}
if(val == '3') {
Speed_need = 0;
Turn_need = 30; //左转
}
if(val == '4') {
Speed_need = 0;
Turn_need = -30; //右转
}
if(val == '5') {
Speed_need = 0; //停止
Turn_need = 0;
}
}
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //获取传感器原始值
Angle_Calcu(); //计算角度
PWM_Calcu(); //PWM输出计算
spcount ++;
if(spcount > 100) {
spcount = 0;
dspcount ++;
//速度与速度积分
speeds = (count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
dspeeds += diff_speeds;
if(dspcount > 5) {
dspeeds_all = dspeeds;
dspeeds = 0;
}
speeds_filter *= 0.85; //一阶互补滤波
speeds_filter += speeds*0.15;
positions += speeds_filter;
positions = constrain(positions, -300, 300); //抗积分饱和
count_left = 0;
count_right = 0;
}
}
void PWM_Calcu(void) {
if (abs(Angle) > 28) {
SetMotorVoltage(0,0); //角度大于30度 停止PWM输出
}else {
//PWM输出计算
Output = Kp*(Angle - Angle0) + Kd*Gyro_y + Ksp*(speeds_filter - Speed_need) + Ksi*positions ;
if(Turn_need == 0){
PWM_left = Output - Kdsp * dspeeds_all;
PWM_right = Output + Kdsp * dspeeds_all;
}
PWM_left = Output - Turn_need;
PWM_right = Output + Turn_need;
if(PWM_left >= 0) {
PWM_left += PWM_left_least;
}else {
PWM_left -= PWM_left_least;
}
if(PWM_right >= 0) {
PWM_right += PWM_right_least;
}else {
PWM_right -= PWM_right_least;
}
SetMotorVoltage(PWM_left, PWM_right);
}
}
还有TT马达还会有齿轮卡榫的情况 就齿轮跟接合 受力不够让它转的情况 解决方法说是要运做不可以停下还是要直接让他一直动不能停下的情况
还有为何里面没有Ki值的程式码呢? 如果额外加了 Kd值不是可以调到小车能额外加东西的吗?
还有序列卓视窗把串口调整的串口关闭 原本的东西跑不出序列卓视窗该显示的东西@.@?
你给的上位机档案有问题 电脑没有cvirte.dll 的档 内建的吗?
打开串口上传后 窗口应该要有东西阿? 可是没东西阿?
目前只找到这些问题 會再回文
感謝
lieak59 发表于 2016-4-13 02:41 static/image/common/back.gif
抱歉大大我有几个问题要问
#56的串口调试工具 sscom4.2
3、millis()函数在哪 我漏看啥?
这个是你调试过程中自己加上去的，最终程序里已经去掉了，做东西是你理解后自己去做，而不是照抄后直接去用的！！！
4、恕我语言能力太差 不太懂将轿车放置于一个固定角度位子(要用墙脚卡车子吗?) 时间要比第3步的还要久?
随便你用什么方式固定小车，用胶粘也行;P
也不太懂 通过串口工具输出“G”或“g”将平衡设定角“setp0”设置到与小车的自然物理平衡角相一致。
看程序中的“G”或“g”命令是什么意思，理解后就明白了。
5、这边不懂得的是不断电意思是都要一直用外接电源了?
不断电的问题，是保证6050传感器输出的角度没有一个不稳定过程，这一切的问题在于你看的程序版本不对，这个说明，对应的程序是：WJY_DIY_v1_2，这是用6050的DMP输出的角度值。
其他问题，自己摸索着解决吧。
本帖最后由 ING1994 于 2016-5-12 15:46 编辑
楼主我想问几个问题
1、kdsp应该怎样设置；
2、dspcount和spcount，是怎样设置的(这个是用来计算速度和路程的时间吗？应该怎样设置)，我的是448光电码盘；
3、怎样计算速度啊（已经知道脉冲数了，但是不知道应该怎样算时间）
4、一阶互补滤波怎么算的速度和路程，没有积分时间吗？
楼主那个i2cdevlib-master，和MPU6050的的网址怎么一样的，不两个文件吗？？？？？？、