首页 › 求助区 › 有两句语言不太懂麻烦高手给解释一下，谢谢！

412823422 发表于 2012-5-23 20:07:09

有两句语言不太懂麻烦高手给解释一下，谢谢！

本帖最后由 412823422 于 2012-5-23 20:44 编辑

sensorAdjusted =sensorAdjusted>0, 70,-70;          在第90行代码

wheel_ls = Torque > 0, wheel_ls,-wheel_ls;      在第103行代码

这个该怎么理解啊，麻烦高手给指点指点，谢谢！//两轮自平衡小车

//IDG330 Mannual, 2mv= 1度/s 的角速率,ad读数1024,3.3v,那么每读数对应 3.223mV,所以每读数对应3.223/2/180*PI= 0.028123弧度/秒
static const double SEMICIRCLE_ARC = 57.29578; /*半圆对应的弧度值*/
static const double GYRO_OPERATOR = 0.028123; /*AD读取的陀螺仪数值对应的弧度算子*/

/*kalman*/
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//注意：dt的取值为kalman滤波器采样时间
double P = {{ 1, 0 },
                  { 0, 1 }};
double Pdot ={ 0,0,0,0};
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;

/*sensor*/
double sensorPort = { 0, 1, 2, 3, 4, 5};/*传感器地址,电路决定.accZ,gyroX, AD1, AD2, AD3, AD4*/
double sensorValue;/*传感器的返回值*/

/*传感器零点 0:Z轴(平行轴) 1:陀螺仪中点*/
double sensorZero = {499,505};
double sensorAdjusted;/*传感器的返回值重整*/
//double provAngle;

/*moto*/
int E1 = 6;
int E2 = 9;
int M1 = 7;
int M2 = 8;
double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35;

/*balance*/
double RATE = { 0,0,0,0};/*公式中的4个变量*/
double K = { 60.45, 1.27, 125, 0.75};/*公式中的4个常量*/
double K_AD = { 1, 1, 1, 1};/*公式中的4个常量*/
double wheel_ls;/*左轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */
double wheel_rs;/*右轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */

double Torque;/*扭矩*/
unsigned int count,count2;
boolean OK=false;//这个是误差达到一定程度后的系统关闭开关.
int bf,X,Y;//从无线端发来的命令

/*kalman*/
/*angle_m:经过atan2(ax,ay)方法计算的偏角,弧度值
gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值
*/
void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dt;
    angle_err = angle_m - angle;
    Pdot=Q_angle - P - P;
    Pdot=- P;
    Pdot=- P;
    Pdot=Q_gyro;
    P += Pdot * dt;
    P += Pdot * dt;
    P += Pdot * dt;
    P += Pdot * dt;
    PCt_0 = C_0 * P;
    PCt_1 = C_0 * P;
    E = R_angle + C_0 * PCt_0;
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;
    t_0 = PCt_0;
    t_1 = C_0 * P;
    P -= K_0 * t_0;
    P -= K_0 * t_1;
    P -= K_1 * t_0;
    P -= K_1 * t_1;
    angle      += K_0 * angle_err;
    q_bias += K_1 * angle_err;
    angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}

void do_balance() {
      
      sensorValue = analogRead(sensorPort);//Z轴加速度
      sensorValue = analogRead(sensorPort);//X轴旋转(陀螺)
      sensorValue = analogRead(sensorPort);//旋钮1
      sensorValue = analogRead(sensorPort);//旋钮2
      sensorValue = analogRead(sensorPort);//旋钮3
      sensorValue = analogRead(sensorPort);//旋钮4
      
      //算出整理过的零点值
      sensorAdjusted = sensorValue - sensorZero;
      if(abs(sensorAdjusted) > 70)//如果倾斜到一定程度,就停机
      {
          OK = false;
          sensorAdjusted =sensorAdjusted>0, 70,-70;
      }
      sensorAdjusted = sensorZero - sensorValue;/*减去初始零点*/
      
      
      /*根据可变电阻改变K的倍数*/
      //deadAreaCompensation = (sensorValue+1)/10.24;
      //K_AD = (sensorValue+1)/256;//102.4;/*值在0.001~10之间变化.在AD=100的时候接近1 */
      //K_AD = (sensorValue+1)/256;//102.4;
      //K_AD = (sensorValue+1)/256;//102.4;
      //K_AD = (sensorValue+1)/256;//102.4;
      
      /*根据上一周期Torqu的值统计轮子转动积分*/
      wheel_ls = Torque > 0, wheel_ls,-wheel_ls;
      wheel_rs = Torque > 0, wheel_rs,-wheel_rs;
      wheel_ls = wheel_rs = 0;
      /*************** balance *********************************************/
      
      /*小车初始状态*/
      
            if(!OK)
            {
            if(abs(sensorAdjusted) <= 3)//小车被推倒平衡位置后才启动电机
            {
                count=0;
                OK = true;
            }
            for(int i = 0; i <7;i++)
            {
                wheel_ls = 0;
                wheel_rs = 0;
            }
            }
      
      
      /*balance*/
      
          /*GYRO_OPERATOR = 0.028123 AD读取的陀螺仪数值对应的弧度算子*/
          Kalman_Filter(atan2(sensorAdjusted, sqrt(10000-sensorAdjusted*sensorAdjusted)), sensorAdjusted * GYRO_OPERATOR);//10000:因为只有一轴加速度计,所以虚拟一个斜边   
      
          RATE = angle * SEMICIRCLE_ARC;//SEMICIRCLE_ARC=57.29578; /*半圆对应的弧度值*/
          RATE = angle_dot * SEMICIRCLE_ARC;
      
      
      /*计算速度 double wheel_ls; 0:编码器累加 1:位移(position) 2:position_dot 3:速度(position_dot_filter) 4:speedNeed 5:turnNeed 6:speedOutPut
      */
      
          wheel_ls *= 0.95;            /*车轮速度滤波,wheel_ls : position_dot_filter*/
          wheel_ls += wheel_ls*0.05; /*wheel_ls : position_dot*/
          wheel_ls += wheel_ls;       /*wheel_ls : position*/
          wheel_ls += wheel_ls;       /*wheel_ls : speedNeed*/
          wheel_ls = max(-50, wheel_ls);
          wheel_ls = min(50, wheel_ls);
      
          RATE = wheel_ls;//速度--滤波过了
          RATE = wheel_ls;//位置
      
      
      /*Torque 综合所有参数算出扭矩*/
      
          Torque = RATE * K * K_AD + RATE * K * K_AD + RATE * K * K_AD + RATE * K* K_AD;
      
          //根据扭矩算轮子的命令值
         wheel_ls = abs(Torque)+ deadAreaCompensation1;//wheel_ls:扭矩输出 deadAreaCompensation1:左轮的死区补偿
      wheel_ls = min(255, wheel_ls);//限制最大扭矩255
          wheel_ls = OK, wheel_ls,0;
      
      wheel_rs = abs(Torque)+ deadAreaCompensation2;
          wheel_rs = min(255, wheel_rs);
          wheel_rs = OK, wheel_rs,0;
      
      
      if(Torque > 0)
      {
          digitalWrite(M2, HIGH);
          digitalWrite(M1, LOW);
      }
      else
      {
          digitalWrite(M2, LOW);
          digitalWrite(M1, HIGH);
      }
      analogWrite(E1, wheel_ls); //PWM调速a==0-255
      analogWrite(E2, wheel_rs);
}
//


void do_msg(){
    if(count%100==0)
    {
      Serial.print("$CLEAR\r\n");
      Serial.print("$GO 1 1\r\n");
      Serial.print("$PRINT ");
   
      Serial.print(wheel_ls);
      Serial.print(" ");
      Serial.print(wheel_ls);
      //Serial.print(" ");
      //Serial.print(RATE);
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      Serial.print("\r\n");
   
      Serial.print("$GO 2 1\r\n");
      Serial.print("$PRINT ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(count);
      Serial.print(wheel_ls);
      Serial.print(" ");
      Serial.print(wheel_rs);
      //Serial.print(count);
      //Serial.print(Torque);
      Serial.print("\r\n");
    }
}
/***************** setup-loop *************************************************/
void setup() {
    count=0;
    X=Y=8;
    bf=-1;
    //init motos
    for (int i = 6; i <= 9; i++) {
      pinMode(i, OUTPUT);
    }
    Serial.begin(9600);//115200);
    analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V
   
    attachInterrupt(0, blinkone, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
    attachInterrupt(1, blinktwo, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
}

void blinkone()//中断函数
{
    wheel_ls ++;
}
void blinktwo()//中断函数
{
    wheel_rs ++;
}

void loop() {
    do_balance();//计算平衡
    do_msg();
    delay(3);
    count++;
    if(count>=60000)
      count=0;
}

Randy 发表于 2012-5-23 20:15:04

麻烦把整个代码贴出来，谢谢

412823422 发表于 2012-5-23 20:40:06

本帖最后由 412823422 于 2012-5-23 20:42 编辑

//两轮自平衡小车

//IDG330 Mannual, 2mv= 1度/s 的角速率,ad读数1024,3.3v,那么每读数对应 3.223mV,所以每读数对应3.223/2/180*PI= 0.028123弧度/秒
static const double SEMICIRCLE_ARC = 57.29578; /*半圆对应的弧度值*/
static const double GYRO_OPERATOR = 0.028123; /*AD读取的陀螺仪数值对应的弧度算子*/

/*kalman*/
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//注意：dt的取值为kalman滤波器采样时间
double P = {{ 1, 0 },
                  { 0, 1 }};
double Pdot ={ 0,0,0,0};
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;

/*sensor*/
double sensorPort = { 0, 1, 2, 3, 4, 5};/*传感器地址,电路决定.accZ,gyroX, AD1, AD2, AD3, AD4*/
double sensorValue;/*传感器的返回值*/

/*传感器零点 0:Z轴(平行轴) 1:陀螺仪中点*/
double sensorZero = {499,505};
double sensorAdjusted;/*传感器的返回值重整*/
//double provAngle;

/*moto*/
int E1 = 6;
int E2 = 9;
int M1 = 7;
int M2 = 8;
double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35;

/*balance*/
double RATE = { 0,0,0,0};/*公式中的4个变量*/
double K = { 60.45, 1.27, 125, 0.75};/*公式中的4个常量*/
double K_AD = { 1, 1, 1, 1};/*公式中的4个常量*/
double wheel_ls;/*左轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */
double wheel_rs;/*右轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */

double Torque;/*扭矩*/
unsigned int count,count2;
boolean OK=false;//这个是误差达到一定程度后的系统关闭开关.
int bf,X,Y;//从无线端发来的命令

/*kalman*/
/*angle_m:经过atan2(ax,ay)方法计算的偏角,弧度值
gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值
*/
void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dt;
    angle_err = angle_m - angle;
    Pdot=Q_angle - P - P;
    Pdot=- P;
    Pdot=- P;
    Pdot=Q_gyro;
    P += Pdot * dt;
    P += Pdot * dt;
    P += Pdot * dt;
    P += Pdot * dt;
    PCt_0 = C_0 * P;
    PCt_1 = C_0 * P;
    E = R_angle + C_0 * PCt_0;
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;
    t_0 = PCt_0;
    t_1 = C_0 * P;
    P -= K_0 * t_0;
    P -= K_0 * t_1;
    P -= K_1 * t_0;
    P -= K_1 * t_1;
    angle      += K_0 * angle_err;
    q_bias += K_1 * angle_err;
    angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}

void do_balance() {
      
      sensorValue = analogRead(sensorPort);//Z轴加速度
      sensorValue = analogRead(sensorPort);//X轴旋转(陀螺)
      sensorValue = analogRead(sensorPort);//旋钮1
      sensorValue = analogRead(sensorPort);//旋钮2
      sensorValue = analogRead(sensorPort);//旋钮3
      sensorValue = analogRead(sensorPort);//旋钮4
      
      //算出整理过的零点值
      sensorAdjusted = sensorValue - sensorZero;
      if(abs(sensorAdjusted) > 70)//如果倾斜到一定程度,就停机
      {
          OK = false;
          sensorAdjusted =sensorAdjusted>0, 70,-70;
      }
      sensorAdjusted = sensorZero - sensorValue;/*减去初始零点*/
      
      
      /*根据可变电阻改变K的倍数*/
      //deadAreaCompensation = (sensorValue+1)/10.24;
      //K_AD = (sensorValue+1)/256;//102.4;/*值在0.001~10之间变化.在AD=100的时候接近1 */
      //K_AD = (sensorValue+1)/256;//102.4;
      //K_AD = (sensorValue+1)/256;//102.4;
      //K_AD = (sensorValue+1)/256;//102.4;
      
      /*根据上一周期Torqu的值统计轮子转动积分*/
      wheel_ls = Torque > 0, wheel_ls,-wheel_ls;
      wheel_rs = Torque > 0, wheel_rs,-wheel_rs;
      wheel_ls = wheel_rs = 0;
      /*************** balance *********************************************/
      
      /*小车初始状态*/
      
            if(!OK)
            {
            if(abs(sensorAdjusted) <= 3)//小车被推倒平衡位置后才启动电机
            {
                count=0;
                OK = true;
            }
            for(int i = 0; i <7;i++)
            {
                wheel_ls = 0;
                wheel_rs = 0;
            }
            }
      
      
      /*balance*/
      
          /*GYRO_OPERATOR = 0.028123 AD读取的陀螺仪数值对应的弧度算子*/
          Kalman_Filter(atan2(sensorAdjusted, sqrt(10000-sensorAdjusted*sensorAdjusted)), sensorAdjusted * GYRO_OPERATOR);//10000:因为只有一轴加速度计,所以虚拟一个斜边   
      
          RATE = angle * SEMICIRCLE_ARC;//SEMICIRCLE_ARC=57.29578; /*半圆对应的弧度值*/
          RATE = angle_dot * SEMICIRCLE_ARC;
      
      
      /*计算速度 double wheel_ls; 0:编码器累加 1:位移(position) 2:position_dot 3:速度(position_dot_filter) 4:speedNeed 5:turnNeed 6:speedOutPut
      */
      
          wheel_ls *= 0.95;            /*车轮速度滤波,wheel_ls : position_dot_filter*/
          wheel_ls += wheel_ls*0.05; /*wheel_ls : position_dot*/
          wheel_ls += wheel_ls;       /*wheel_ls : position*/
          wheel_ls += wheel_ls;       /*wheel_ls : speedNeed*/
          wheel_ls = max(-50, wheel_ls);
          wheel_ls = min(50, wheel_ls);
      
          RATE = wheel_ls;//速度--滤波过了
          RATE = wheel_ls;//位置
      
      
      /*Torque 综合所有参数算出扭矩*/
      
          Torque = RATE * K * K_AD + RATE * K * K_AD + RATE * K * K_AD + RATE * K* K_AD;
      
          //根据扭矩算轮子的命令值
         wheel_ls = abs(Torque)+ deadAreaCompensation1;//wheel_ls:扭矩输出 deadAreaCompensation1:左轮的死区补偿
      wheel_ls = min(255, wheel_ls);//限制最大扭矩255
          wheel_ls = OK, wheel_ls,0;
      
      wheel_rs = abs(Torque)+ deadAreaCompensation2;
          wheel_rs = min(255, wheel_rs);
          wheel_rs = OK, wheel_rs,0;
      
      
      if(Torque > 0)
      {
          digitalWrite(M2, HIGH);
          digitalWrite(M1, LOW);
      }
      else
      {
          digitalWrite(M2, LOW);
          digitalWrite(M1, HIGH);
      }
      analogWrite(E1, wheel_ls); //PWM调速a==0-255
      analogWrite(E2, wheel_rs);
}
//


void do_msg(){
    if(count%100==0)
    {
      Serial.print("$CLEAR\r\n");
      Serial.print("$GO 1 1\r\n");
      Serial.print("$PRINT ");
   
      Serial.print(wheel_ls);
      Serial.print(" ");
      Serial.print(wheel_ls);
      //Serial.print(" ");
      //Serial.print(RATE);
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      Serial.print("\r\n");
   
      Serial.print("$GO 2 1\r\n");
      Serial.print("$PRINT ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(" ");
      //Serial.print(K*K_AD);
      //Serial.print(count);
      Serial.print(wheel_ls);
      Serial.print(" ");
      Serial.print(wheel_rs);
      //Serial.print(count);
      //Serial.print(Torque);
      Serial.print("\r\n");
    }
}
/***************** setup-loop *************************************************/
void setup() {
    count=0;
    X=Y=8;
    bf=-1;
    //init motos
    for (int i = 6; i <= 9; i++) {
      pinMode(i, OUTPUT);
    }
    Serial.begin(9600);//115200);
    analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V
   
    attachInterrupt(0, blinkone, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
    attachInterrupt(1, blinktwo, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
}

void blinkone()//中断函数
{
    wheel_ls ++;
}
void blinktwo()//中断函数
{
    wheel_rs ++;
}

void loop() {
    do_balance();//计算平衡
    do_msg();
    delay(3);
    count++;
    if(count>=60000)
      count=0;
}

GeMarK 发表于 2012-5-23 21:41:32

sensorAdjusted = sensorAdjusted>0, 70,-70;

比如说 在执行这个语句之前sensorAdjusted里面的值如果大于0，那么sensorAdjusted就会被重新复制为1，反之返回0

> < 就是关系运算符，关系运算符的返回结果如无意外都是0或者1，对于C/C++来说，假就是0，非0就是真。关系运算符的结果其实就是一个逻辑结果。明白否？

当然我是按C/C++的来解释，如果解释错了，请高手指正。

GeMarK 发表于 2012-5-23 21:43:45

你去看看arduino的手册，看看><= >= <= 是否就是关系运算符，如果没有其他解释，那么我上面说应该就是你想要的答案了。

哦，上面打错字了，不是复制，是赋值。

GeMarK 发表于 2012-5-23 21:47:57

程序如果这样写，你可能会比较明白

sensorAdjusted = 50;
if(sensorAdjusted = sensorAdjusted>0)
{
printf("sensorAdj>0"\N);
}
else
{
printf("sensorAdj<0"\N)
}

输出结果：sensorAdj>0

GeMarK 发表于 2012-5-23 21:51:49

关系运算符要优先与赋值，所以程序执行到这里时，先判断大小，并将0或者1，假或者非假赋值给前者，然后在进行判断。
希望我的解释可以帮你解惑。

yyy_zc 发表于 2012-5-25 22:37:34

GeMarK 发表于 2012-5-23 21:47 static/image/common/back.gif
程序如果这样写，你可能会比较明白

sensorAdjusted = 50;


语法和书写问题，书写简单了，看的人看不懂了。所以要规范书写

Arduino_CAO 发表于 2012-5-26 10:59:05

记得条件表达式是这样的格式(条件表达式1 ? 条件表达式2 : 条件表达式3)；arduino改了这个写法？

查看完整版本: 有两句语言不太懂麻烦高手给解释一下，谢谢！