我正在尝试构建一个基于树莓派的四轴飞行器。到目前为止，我已经成功地连接了所有硬件，并且我编写了一个在低油门时相当稳定的 PID 控制器。问题是在更高的油门下，四轴飞行器开始颠簸和抽搐。我什至还没能把它弄好，我所有的测试都是在测试台上完成的。这是我的代码有问题，还是电机坏了？非常感谢任何建议。
到目前为止，这是我的代码：
QuadServer.java：
package com.zachary.quadserver;
import java.net.*;
import java.io.*;
import java.util.*;
import se.hirt.pi.adafruit.pwm.PWMDevice;
import se.hirt.pi.adafruit.pwm.PWMDevice.PWMChannel;
public class QuadServer {
private static Sensor sensor = new Sensor();
private final static int FREQUENCY = 490;
private static double PX = 0;
private static double PY = 0;
private static double IX = 0;
private static double IY = 0;
private static double DX = 0;
private static double DY = 0;
private static double kP = 1.3;
private static double kI = 2;
private static double kD = 0;
private static long time = System.currentTimeMillis();
private static double last_errorX = 0;
private static double last_errorY = 0;
private static double outputX;
private static double outputY;
private static int val[] = new int[4];
private static int throttle;
static double setpointX = 0;
static double setpointY = 0;
static long receivedTime = System.currentTimeMillis();
public static void main(String[] args) throws IOException, NullPointerException {
PWMDevice device = new PWMDevice();
device.setPWMFreqency(FREQUENCY);
PWMChannel BR = device.getChannel(12);
PWMChannel TR = device.getChannel(13);
PWMChannel TL = device.getChannel(14);
PWMChannel BL = device.getChannel(15);
DatagramSocket serverSocket = new DatagramSocket(8080);
Thread read = new Thread(){
public void run(){
while(true) {
try {
byte receiveData[] = new byte[1024];
DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
serverSocket.receive(receivePacket);
String message = new String(receivePacket.getData());
throttle = (int)(Integer.parseInt((message.split("\\s+")[4]))*12.96)+733;
setpointX = Integer.parseInt((message.split("\\s+")[3]))-50;
setpointY = Integer.parseInt((message.split("\\s+")[3]))-50;
receivedTime = System.currentTimeMillis();
} catch (IOException e) {
e.printStackTrace();
}
}
}
};
read.start();
while(true)
{
Arrays.fill(val, calculatePulseWidth((double)throttle/1000, FREQUENCY));
double errorX = -sensor.readGyro(0)-setpointX;
double errorY = sensor.readGyro(1)-setpointY;
double dt = (double)(System.currentTimeMillis()-time)/1000;
double accelX = sensor.readAccel(0);
double accelY = sensor.readAccel(1);
double accelZ = sensor.readAccel(2);
double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));
double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));
if(dt > 0.01)
{
PX = errorX;
PY = errorY;
IX += errorX*dt;
IY += errorY*dt;
IX = 0.95*IX+0.05*accelAngleX;
IY = 0.95*IY+0.05*accelAngleY;
DX = (errorX - last_errorX)/dt;
DY = (errorY - last_errorY)/dt;
outputX = kP*PX+kI*IX+kD*DX;
outputY = kP*PY+kI*IY+kD*DY;
time = System.currentTimeMillis();
}
System.out.println(setpointX);
add(-outputX+outputY, 0);
add(-outputX-outputY, 1);
add(outputX-outputY, 2);
add(outputX+outputY, 3);
//System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);
if(System.currentTimeMillis()-receivedTime < 1000)
{
BR.setPWM(0, val[0]);
TR.setPWM(0, val[1]);
TL.setPWM(0, val[2]);
BL.setPWM(0, val[3]);
} else
{
BR.setPWM(0, 1471);
TR.setPWM(0, 1471);
TL.setPWM(0, 1471);
BL.setPWM(0, 1471);
}
}
}
private static void add(double value, int i)
{
value = calculatePulseWidth(value/1000, FREQUENCY);
if(val[i]+value > 1471 && val[i]+value < 4071)
{
val[i] += value;
}else if(val[i]+value < 1471)
{
//System.out.println("low");
val[i] = 1471;
}else if(val[i]+value > 4071)
{
//System.out.println("low");
val[i] = 4071;
}
}
private static int calculatePulseWidth(double millis, int frequency) {
return (int) (Math.round(4096 * millis * frequency/1000));
}
}
传感器.java：
package com.zachary.quadserver;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;
import java.net.*;
import java.io.*;
public class Sensor {
static I2CDevice sensor;
static I2CBus bus;
static byte[] accelData, gyroData;
static long accelCalib[] = new long[3];
static long gyroCalib[] = new long[3];
static double gyroX = 0;
static double gyroY = 0;
static double gyroZ = 0;
static double accelX;
static double accelY;
static double accelZ;
static double angleX;
static double angleY;
static double angleZ;
public Sensor() {
//System.out.println("Hello, Raspberry Pi!");
try {
bus = I2CFactory.getInstance(I2CBus.BUS_1);
sensor = bus.getDevice(0x68);
sensor.write(0x6B, (byte) 0x0);
sensor.write(0x6C, (byte) 0x0);
System.out.println("Calibrating...");
calibrate();
Thread sensors = new Thread(){
public void run(){
try {
readSensors();
} catch (IOException e) {
System.out.println(e.getMessage());
}
}
};
sensors.start();
} catch (IOException e) {
System.out.println(e.getMessage());
}
}
private static void readSensors() throws IOException {
long time = System.currentTimeMillis();
long sendTime = System.currentTimeMillis();
while (true) {
accelData = new byte[6];
gyroData = new byte[6];
int r = sensor.read(0x3B, accelData, 0, 6);
accelX = (((accelData[0] << 8)+accelData[1]-accelCalib[0])/16384.0)*9.8;
accelY = (((accelData[2] << 8)+accelData[3]-accelCalib[1])/16384.0)*9.8;
accelZ = ((((accelData[4] << 8)+accelData[5]-accelCalib[2])/16384.0)*9.8)+9.8;
accelZ = 9.8-Math.abs(accelZ-9.8);
double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));
double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));
//System.out.println((int)gyroX+", "+(int)gyroY);
//System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);
r = sensor.read(0x43, gyroData, 0, 6);
if(System.currentTimeMillis()-time >= 5)
{
gyroX = (((gyroData[0] << 8)+gyroData[1]-gyroCalib[0])/131.0);
gyroY = (((gyroData[2] << 8)+gyroData[3]-gyroCalib[1])/131.0);
gyroZ = (((gyroData[4] << 8)+gyroData[5]-gyroCalib[2])/131.0);
angleX += gyroX*(System.currentTimeMillis()-time)/1000;
angleY += gyroY*(System.currentTimeMillis()-time)/1000;
angleZ += gyroZ;
angleX = 0.95*angleX + 0.05*accelAngleX;
angleY = 0.95*angleY + 0.05*accelAngleY;
time = System.currentTimeMillis();
}
//System.out.println((int)angleX+", "+(int)angleY);
//System.out.println((int)accelAngleX+", "+(int)accelAngleY);
}
}
public static void calibrate() throws IOException {
int i;
for(i = 0; i < 3000; i++)
{
accelData = new byte[6];
gyroData = new byte[6];
int r = sensor.read(0x3B, accelData, 0, 6);
accelCalib[0] += (accelData[0] << 8)+accelData[1];
accelCalib[1] += (accelData[2] << 8)+accelData[3];
accelCalib[2] += (accelData[4] << 8)+accelData[5];
r = sensor.read(0x43, gyroData, 0, 6);
gyroCalib[0] += (gyroData[0] << 8)+gyroData[1];
gyroCalib[1] += (gyroData[2] << 8)+gyroData[3];
gyroCalib[2] += (gyroData[4] << 8)+gyroData[5];
try {
Thread.sleep(1);
} catch (Exception e){
e.printStackTrace();
}
}
gyroCalib[0] /= i;
gyroCalib[1] /= i;
gyroCalib[2] /= i;
accelCalib[0] /= i;
accelCalib[1] /= i;
accelCalib[2] /= i;
System.out.println(gyroCalib[0]+", "+gyroCalib[1]+", "+gyroCalib[2]);
}
public double readAngle(int axis)
{
switch (axis)
{
case 0:
return angleX;
case 1:
return angleY;
case 2:
return angleZ;
}
return 0;
}
public double readGyro(int axis)
{
switch (axis)
{
case 0:
return gyroX;
case 1:
return gyroY;
case 2:
return gyroZ;
}
return 0;
}
public double readAccel(int axis)
{
switch (axis)
{
case 0:
return accelX;
case 1:
return accelY;
case 2:
return accelZ;
}
return 0;
}
}