我试图建立一个PID控制器的赛格威型车辆。我运行代码,它将在几秒钟后停止工作。我正在使用一个sparkfun 9d0f IMU与Arduino uno。我猜是某个地方溢出了或是死循环了有什么想法吗?会不会是从惯性测量装置上得到的?
/******************************
* segBoard Attitude Control 2.0
* this is the second version of the attitude control code for the
* spring 2015 400d segBoard project. this code uses the sparkfun
* 9dof to gather data about the attitude of the board and convert
* it to PID for controlling the motor.
*
* Written by: Antonio Cole 2/29/15
*
*/
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM 0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G 0x6B // Would be 0x6A if SDO_G is LOW
// Create an instance of the LSM9DS0 library called `dof` the
// parameters for this constructor are:
// [SPI or I2C Mode declaration],[gyro I2C address],[xm I2C add.]
LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
const int leftPin = 9;
const int rightPin = 10;
const int samples = 60;
int pitchControl = 0;
int index = 0;
int pitch = 0;
float angle = 0;
float accelX[samples];
float accelY[samples];
float accelZ[samples];
float accelXaverage = 0;
float accelYaverage = 0;
float accelZaverage = 0;
void setup()
{
pinMode(leftPin, OUTPUT);
pinMode(rightPin, OUTPUT);
Serial.begin(115200); // Start serial at 115200 bps
// Use the begin() function to initialize the LSM9DS0 library.
// You can either call it with no parameters (the easy way):
uint16_t status = dof.begin();
/*for(int i = 0; i < 6 i++){
dof.readAccel();
}
delay(1);
*/
Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
Serial.println(status, HEX);
Serial.println("Should be 0x49D4");
Serial.println();
}
void loop(){
dof.readAccel();
accelX[index] = dof.calcAccel(dof.ax) - 0.01;
accelY[index] = dof.calcAccel(dof.ay) - 0.01;
accelZ[index] = dof.calcAccel(dof.az) - 0.02;
float accelXsum = 0;
float accelYsum = 0;
float accelZsum = 0;
for(int i = 0; i < samples; i++) {
accelXsum = accelXsum + accelX[i];
accelYsum = accelYsum + accelY[i];
accelZsum = accelZsum + accelZ[i];
}
accelXaverage = accelXsum / samples;
accelYaverage = accelYsum / samples;
accelZaverage = accelZsum / samples;
angle = atan2(accelXaverage, sqrt(accelYaverage * accelYaverage)
+ (accelZaverage * accelZaverage))*100;
pitch = constrain(angle, -15, 15);
if(pitch < 0){
pitchControl = map(pitch, -15, 0, 255, 0);
analogWrite(rightPin, 0);
analogWrite(leftPin, pitchControl);
}
else if(pitch > 0){
pitchControl = map(pitch, 0, 15, 0, 255);
analogWrite(leftPin, 0);
analogWrite(rightPin, pitchControl);
}
else{
analogWrite(leftPin, 0);
analogWrite(rightPin, 0);
}
Serial.print(index);
Serial.print(";");
Serial.println(pitchControl);
index++;
if(index == samples){
index = 0;
}
}
1条答案
按热度按时间7xllpg7q1#
在对这个系统一无所知的情况下,您需要应用标准的调试技术来隔离问题。
把所有的东西都拉出来,这样你就有了空的setup()和main()函数。也许可以加上print语句。看看能不能成功然后继续添加代码,直到它崩溃。最后补充的是问题。