现在就是向一个方向小车倾斜的时候可以,但是由于l298n的顺时针和逆时针的高低电平设定好了,所以无论怎么调也都是一个方向,从PID输出的Output值虽然有正负,给到IN1和IN2的时候正负的转速值不会改变电机的方向。。。求教啊~怎么能变换两个方向?(用了IF语句也不行。。)
unsigned char Re_buf[11],counter=0;
unsigned char sign=0;
float a[3],w[3],Angle[3],T;
short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
int Pwm_out = 0;
int Turn_Need = 0;
int Speed_Need = 0;
int speed_output_R , speed_output_L;
int speedoutputL = 13;
int speedoutputR = 8;
int L1 =12;
int L2 =11;
int R1 =10;
int R2 =9;
long count_left = 0;
long count_right = 0;
float speeds , speeds_filter, positions;
float diff_speeds,diff_speeds_all;
void setup() {
// initialize serial:
Serial.begin(115200);
Serial3.begin(115200);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
pinMode(11,OUTPUT);
pinMode(12,OUTPUT);
pinMode(13,OUTPUT);
}
void loop()
{
serialEvent();
Serial.print(Pwm_out);
Serial.println(",");
Serial.print("angle:");
Serial.print(Angle[0]);
Serial.println(",");
Serial.print("rad");
Serial.print(w[0]);
Serial.println(",");
float Kap = 4.8; //
float Kad = 10; //
float Ksp = 2.8; //
float Ksi = 0.11; //
speeds=(count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
diff_speeds_all += diff_speeds;
speeds_filter *=0.85; //一阶互补滤波
speeds_filter +=speeds*0.15;
positions += speeds_filter;
positions += Speed_Need;
positions = constrain(positions, -2300, 2300);//抗积分饱和
Pwm_out = Kap*Angle[0] + Kad*w[0] + Ksp*speeds_filter + Ksi*positions ; //PID控制器
if(Turn_Need == 0)
{
speed_output_R = int(Pwm_out - diff_speeds_all);
speed_output_L = int(Pwm_out + diff_speeds_all);
}
speed_output_R = int(Pwm_out + Turn_Need);
speed_output_L = int(Pwm_out - Turn_Need);
digitalWrite(9,1);
digitalWrite(10,0);
digitalWrite(11,0);
digitalWrite(12,1);
analogWrite(speedoutputL,speed_output_L);
analogWrite(speedoutputR,speed_output_R);
count_left = 0;
count_right = 0;
}
void serialEvent() {
while (Serial3.available()) {
//char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
Re_buf[counter]=(unsigned char)Serial3.read();
if(counter==0&&Re_buf[0]!=0x55) return; //第0号数据不是帧头
counter++;
if(counter==11) //接收到11个数据
{
counter=0; //重新赋值,准备下一帧数据的接收
switch(Re_buf [1])
{
case 0x51:
a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
a[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
a[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;
break;
case 0x52:
w[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
w[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
w[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
break;
case 0x53:
Angle[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
Angle[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
Angle[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
T = float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25
sign=1;
break;
}
}
}
}
// Serial.print(Angle[0]); //主调被调量
// Serial.print(',');
// Serial.print(speeds_filter);//副调被调量
// Serial.print(',');
// Serial.println(Pwm_out); //输出量
|