作为一个小总结吧…
学习是从上个寒假开始的, 熊老师让我帮他弄那个轮椅控制的事情= =当时每个星期天都跑了过去帮忙, 连续弄了好几个星期吧, 最后快到期末基本弄成了.
然后这两天帮他们把他们后续的东西大致给弄完了, 然后晚上感觉在那边睡着不是很踏实, 就今天跑回来了, 毕竟后面的任务我在家也可以完成了, 在那边确实呆着不是很舒服, 洗澡什么的也不方便.
帮那个同学弄了一下震荡检测的, 虽然这个算法我觉得还是需要在考虑下或者参数需要调好点, 不过最后结果看起来还是可以的.
在那边主要帮忙了三个东西, 一个摇晃检测, 一个移动动态控制, 还有一个GPRS数据传输的东西.
不多说代码如下
摇晃检测
#include <CurieIMU.h>
#include <MadgwickAHRS.h>
Madgwick filter; // initialise Madgwick object
int ax, ay, az;
int gx, gy, gz;
float yaw[30];
float pitch[30];
float roll[30];
int factor = 800; // variable by which to divide gyroscope values, used to control sensitivity
// note that an increased baud rate requires an increase in value of factor
int calibrateOffsets = 1; // int to determine whether calibration takes place or not
void setup() {
// initialize Serial communication
Serial.begin(9600);
// initialize device
CurieIMU.begin();
if (calibrateOffsets == 1) {
// use the code below to calibrate accel/gyro offset values
Serial.println("Internal sensor offsets BEFORE calibration...");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
Serial.println("");
// To manually configure offset compensation values, use the following methods instead of the autoCalibrate...() methods below
// CurieIMU.setGyroOffset(X_AXIS, 220);
// CurieIMU.setGyroOffset(Y_AXIS, 76);
// CurieIMU.setGyroOffset(Z_AXIS, -85);
// CurieIMU.setAccelerometerOffset(X_AXIS, -76);
// CurieIMU.setAccelerometerOffset(Y_AXIS, -235);
// CurieIMU.setAccelerometerOffset(Z_AXIS, 168);
//IMU device must be resting in a horizontal position for the following calibration procedure to work correctly!
Serial.print("Starting Gyroscope calibration...");
CurieIMU.autoCalibrateGyroOffset();
Serial.println(" Done");
Serial.print("Starting Acceleration calibration...");
CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
Serial.println(" Done");
Serial.println("Internal sensor offsets AFTER calibration...");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.println("");
}
}
int k = 0;
int flag = 0;
float roll3[3];
float pitch3[3];
int sign = 0;
void readFirstMotion(){
for(int i = 0;i<30;i++){
CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);
filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
roll[i] = filter.getRoll();
pitch[i] = filter.getPitch();
}
}
void refreshMotion(){
for(int i = 0;i<29;i++){
roll[i] = roll[i+1];
pitch[i] = pitch[i+1];
}
CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);
filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
roll[29] = filter.getRoll();
pitch[29] = filter.getPitch();
}
int var(){
refreshMotion();
for(int i = 0;i<29;i++){
if(i % 10 == 0 && i <= 20){
float roll_mean = 0;
float pitch_mean = 0;
for(int k = i;k<i+10;k++){
roll_mean += roll[k];
pitch_mean += pitch[k];
}
int m = i/10;
roll3[m] = roll_mean/10;
pitch3[m] = pitch_mean/10;
}
}
if(abs((roll3[1] - roll3[2]) >= 0.5) || (abs(roll3[2] - roll[3]) >= 0.5) || abs((pitch3[1] - pitch3[2]) >= 0.5) || (abs(pitch3[2] - pitch[3]) >= 0.5))
return 1;
else
return 0;
}
int judge(){
int count = 0;
for(int i = 0; i<5000; i++){
int sign = var();
if(sign == 1){
count++;
}
}
if(count >= 4000){
return 1;
}else
return 0;
}
void loop() {
if(flag == 0){
readFirstMotion();
flag++;
}
sign = judge();
if (sign == 0){
Serial.println("situation is OK");
}else{
Serial.println("bad situation");
}
}