void* KeyBoard(void*)
{
char keychar;
while(1)
{
keychar = getchar();
if (keychar == 'q')
{
Default_Acc += 0.01;
if (Default_Acc > MAX_ACC)
{
Default_Acc = MAX_ACC;
}
}
else if (keychar == 'a')
{
Default_Acc -= 0.01;
if (Default_Acc < 0.02)
{
Default_Acc = 0.02;
}
}
else if (keychar == 'e')
{
Default_Acc = 0.05;
delay(200);
Default_Acc = 0.03;
delay(200);
}
else if (keychar == 'r')
{
Default_Acc = 0.05;
delay(200);
Default_Acc = 0.03;
delay(200);
PWMOut(PinNumber1,0.03);
PWMOut(PinNumber2,0.03);
PWMOut(PinNumber3,0.03);
PWMOut(PinNumber4,0.03);
return 0;
}
else if (keychar == 'w')
{
Default_Acc = 0.5;
}
}
}
int TIM1_UP_IRQHandler(void)
{
static int SysTime = 0;
if(TIM1->SR&0X0001)//5ms定时中断
{
TIM1->SR&=~(1<<0); //===清除定时器1中断标志位
Flag_Target=!Flag_Target;
if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
{
Get_Angle(Way_Angle); //===更新姿态
return 0;
} //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
// Encoder_Left=Read_Encoder(2); //===读取编码器的值，
// Encoder_Right=Read_Encoder(3); //===读取编码器的值
Get_Angle(Way_Angle); //===更新姿态
Led_Flash(100); //===LED闪烁;指示单片机正常运行
Key(); //===扫描按键状态 单击双击可以改变小车运行状态
// Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===平衡PID控制
// Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
// Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制
// Moto1=Balance_Pwm+Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM
// Moto2=Balance_Pwm+Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM
SysTime += 10;
// Moto1 = PWMOut(waveOut( 0.5, 1, 0, 0, SysTime) );
// Moto2 = PWMOut(waveOut( 0.5, 1, (30/180*PI), 0, SysTime));
Moto1 = PWMOut(&motorCMD[0],SysTime);
Moto2 = PWMOut(&motorCMD[1],SysTime);
Moto3 = PWMOut(&motorCMD[2],SysTime);
Moto4 = PWMOut(&motorCMD[3],SysTime);
// Xianfu_Pwm(); //===PWM限幅
// if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
//printf("PWMOUT A : %d",Moto1 );
//printf("PWMOUT B : %d",Moto2 );
// if(SysTime%1000==0)printf("PWMOUT A : %d",Moto1 );
Set_Pwm(Moto1,Moto2,Moto3,Moto4); //===赋值给PWM寄存器
}
return 0;
}
void* gyro_acc(void*)
{
//float kp = 0.00375,ki = 0.0000,kd = 0.00076;
float kp = 0.0068,ki = 0.000,kd = 0.0018;
//0030 0088 0014 有偏角 p0.0031偏角更大 0.0029也是 i=0 小偏角 p0.00305 d0.00143 不错 i0.0005 偏角变大
//0032 0017
float pregyro =0;
float desired = 0;
//double error;
float integ=0;//integral积分参数
float iLimit =8 ;
float deriv=0;//derivative微分参数
float prevError=0;
float lastoutput=0;
//float Piddeadband=0.3;
// initialize device
printf("Initializing I2C devices...\n");
mpu.initialize();
// verify connection
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true);
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
}
/*****************************************************/
while(1)
{
if (START_FLAG == 0)
{
delay(200);
}
if (START_FLAG == 1)
{
break;
}
}
delay(50);
for(;;)
{
if (!dmpReady) return 0;
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (fifoCount >= 42)
{
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//printf("ypr %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
Angle[2] = ypr[0] * 180/M_PI;
Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
Angle[0] = ypr[2] * 180/M_PI;//此为Roll
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
//printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z);
AngleSpeed[0] = aaWorld.x;
AngleSpeed[1] = aaWorld.y;
AngleSpeed[2] = aaWorld.z;
/****************************读取完毕*********************************/
error = desired - Angle[0];//偏差：期望-测量值
All_Count = All_Count + 1;
error = error * 0.88 + prevError * 0.12;
/*
if (fabs(prevError - error ) > 12)
{
error = prevError;
}*/
integ += error * IMU_UPDATE_DT;//偏差积分，IMU_UPDATE_DT也就是每调整漏斗大小的步辐
if (integ >= iLimit)//作积分限制
{
integ = iLimit;
}
else if (integ < -iLimit)
{
integ = -iLimit;
}
deriv = (error - prevError) / IMU_UPDATE_DT;//微分 应该可用陀螺仪角速度代替
AngleSpeed[0] = deriv;
if (fabs(deriv) < 20 )
{
if (fabs(deriv) < 10 )
{
deriv = deriv * 0.8;
}
else
{
deriv = deriv * 0.9;
}
}
//if(deriv
//roll.deriv = -gyro;//注意是否跟自己的参数方向相反，不然会加剧振荡
//deriv = -AngleSpeed[0];
/*
if (fabs(pregyro - deriv) > 20)
{
deriv = deriv * 0.5 + pregyro * 0.5;
}
*/
output = (kp * error) + (ki * integ) + (kd * deriv);
prevError = error;//更新前一次偏差
pregyro = deriv;
if (output > 0.16)
{
output = 0.16;
}
if (output < -0.16)
{
output = -0.16;
}
Pid_Roll = output;
//output = output * 0.9 + lastoutput * 0.1;
if (fabs(error) < 0.3 )
{
output = lastoutput * 0.5;
}
lastoutput = output;
DutyCycle[0] = Default_Acc - output;
DutyCycle[1] = Default_Acc - output;
//DutyCycle[0] = Default_Acc;
//DutyCycle[1] = Default_Acc;
DutyCycle[2] = Default_Acc + output;
DutyCycle[3] = Default_Acc + output;
//DutyCycle[2] = Default_Acc;
//DutyCycle[3] = Default_Acc;
PWMOut(PinNumber1,DutyCycle[0]);
PWMOut(PinNumber2,DutyCycle[1]);
PWMOut(PinNumber3,DutyCycle[2]);
PWMOut(PinNumber4,DutyCycle[3]);
}
}
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
getchar();
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
//Pid_Pitch = PidUpdate(Angle[1],-1.6,AngleSpeed[1]);
//Pid_Roll = PidUpdate_roll(Angle[0],0,AngleSpeed[0]);
TimeNow = millis();
system("clear");
//delay(100);
printf("Pid_Roll:%.4f error %.3f All_Count: %d time:%d\n",Pid_Roll,error,All_Count,TimeNow - TimeStart);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro：Pitch：%.2f roll :%.2f\n",Default_Acc,AngleSpeed[1],AngleSpeed[0]);
fflush(stdout);
//DutyCycle[3] = Default_Acc + Pid_Pitch - Pid_Roll;//+yaw
//DutyCycle[2] = Default_Acc - Pid_Pitch + Pid_Roll;//+yaw
//DutyCycle[1] = Default_Acc + Pid_Pitch + Pid_Roll;//-yaw
//DutyCycle[0] = Default_Acc - Pid_Pitch - Pid_Roll;//-yaw
}
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
Pid_Inital();
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
TimeNow = millis();
system("clear");
printf("Pid_Roll:%.4f pid_error:%.3f pid_pError:%.3f pregyro %.3f All_Count: %d",Pid_Roll,pid_error,Roll_PError,pregyro,All_Count);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro： roll :%.2f\n",Default_Acc,AngleSpeed[0]);
fflush(stdout);
}
}
void* gyro_acc(void*)
{
int i = 0;
// initialize device
printf("Initializing I2C devices...\n");
mpu.initialize();
// verify connection
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true);
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
return 0;
}
/*****************************************************/
while(1)
{
if (START_FLAG == 0)
{
delay(200);
}
if (START_FLAG == 1)
{
break;
}
}
delay(50);
for(;;)
{
if (!dmpReady) return 0;
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (fifoCount >= 42)
{
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//printf("ypr %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
Angle[2] = ypr[0] * 180/M_PI;
Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
Angle[0] = ypr[2] * 180/M_PI;//此为Roll
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
/*
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
//printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z);
//AngleSpeed[0] = aaWorld.x;
//AngleSpeed[1] = aaWorld.y;
//AngleSpeed[2] = aaWorld.z;
*/
/****************************读取完毕*********************************/
if (Inital <= 300)
{
Inital ++;
if (Inital % 98 == 1)
{
Inital_Roll[i] = Angle[0];
Inital_Pitch[i] = Angle[1];
Inital_Yaw[i] = Angle[2];
printf("Roll:%.2f Pitch:%.2f Yaw:%.2f",Inital_Roll[i],Inital_Pitch[i],Inital_Yaw[i]);
i++;
printf("%d\n",Inital);
fflush(stdout);
if (i == 3)
{
Inital_Yaw[3] = (Inital_Yaw[0] + Inital_Yaw[1] + Inital_Yaw[2]) / 3;
Inital_Roll[3] =(Inital_Roll[0] + Inital_Roll[1] + Inital_Roll[2]) / 3;
Inital_Pitch[3] = (Inital_Pitch[0] + Inital_Pitch[1] + Inital_Pitch[2]) / 3;
}
}
}
else
{
Pid_Roll = Pid_Calc_R(Roll_Suit,Angle[0]);
Pid_Pitch = Pid_Calc_P(Pitch_Suit,Angle[1]);
Pid_Yaw = Pid_Calc_Y(Yaw_Suit,Angle[2],Inital_Yaw[3]);
All_Count = All_Count + 1;
DutyCycle[0] = Default_Acc - Pid_Roll - Pid_Pitch; //- Pid_Yaw;
DutyCycle[1] = Default_Acc - Pid_Roll + Pid_Pitch; //+ Pid_Yaw;
//DutyCycle[0] = Default_Acc;
//DutyCycle[1] = Default_Acc;
DutyCycle[2] = Default_Acc + Pid_Roll - Pid_Pitch; //+ Pid_Yaw;
DutyCycle[3] = Default_Acc + Pid_Roll + Pid_Pitch; //- Pid_Yaw;
//DutyCycle[2] = Default_Acc;
//DutyCycle[3] = Default_Acc;
PWMOut(PinNumber1,DutyCycle[0]);
PWMOut(PinNumber2,DutyCycle[1]);
PWMOut(PinNumber3,DutyCycle[2]);
PWMOut(PinNumber4,DutyCycle[3]);
}
}
}
}
