mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 21:54:12 +05:30
minor changes
This commit is contained in:
parent
da09baacd2
commit
d790520ddc
@ -10,21 +10,8 @@
|
||||
#include "motor_driver.h"
|
||||
#include "encoder_driver.h"
|
||||
#include "diff_controller.h"
|
||||
#include <Wire.h>
|
||||
#include "I2Cdev.h"
|
||||
#include "AK8975.h"
|
||||
#include "MPU6050.h"
|
||||
#include "Kalman.h"
|
||||
|
||||
|
||||
Kalman filter;
|
||||
AK8975 mag(0x0C);
|
||||
MPU6050 accelgyro;
|
||||
float batt_lvl;
|
||||
int16_t gx;
|
||||
int16_t mx, my, mz;
|
||||
int dt;
|
||||
|
||||
|
||||
#define PID_RATE 30 // Hz
|
||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||
@ -39,7 +26,6 @@ char argv1[16];
|
||||
char argv2[16];
|
||||
long arg1;
|
||||
long arg2;
|
||||
float heading = 0.0;
|
||||
|
||||
void resetCommand() {
|
||||
cmd = NULL;
|
||||
@ -128,9 +114,6 @@ int runCommand() {
|
||||
case BATTERY_LVL:
|
||||
Serial.println((int)(batt_lvl * 100));
|
||||
break;
|
||||
case IMU:
|
||||
Serial.println((int)(heading * 100));
|
||||
break;
|
||||
default:
|
||||
Serial.println("Invalid Command");
|
||||
break;
|
||||
@ -139,29 +122,12 @@ int runCommand() {
|
||||
|
||||
/* Setup function--runs once at startup. */
|
||||
void setup() {
|
||||
Wire.setClock(400000);
|
||||
Wire.begin();
|
||||
Serial.begin(BAUDRATE);
|
||||
|
||||
accelgyro.initialize();
|
||||
accelgyro.setI2CBypassEnabled(true);
|
||||
mag.initialize();
|
||||
mag.testConnection();
|
||||
delay(100);
|
||||
mag.getHeading(&my, &mx, &mz);
|
||||
delay(10);
|
||||
mag.getHeading(&my, &mx, &mz);
|
||||
|
||||
heading = atan2(-mz, my) * RAD_TO_DEG;
|
||||
|
||||
filter.setQangle(0.0001);
|
||||
filter.setQbias(0.003);
|
||||
filter.setRmeasure(0.05);
|
||||
filter.setAngle(heading);
|
||||
|
||||
pinMode(A0,INPUT);
|
||||
digitalWrite(A0,1);
|
||||
batt_lvl = analogRead(A0);
|
||||
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
||||
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
||||
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
||||
@ -186,33 +152,10 @@ void setup() {
|
||||
|
||||
}
|
||||
|
||||
uint32_t timer;
|
||||
void loop() {
|
||||
|
||||
gx = accelgyro.getRotationX();
|
||||
|
||||
mag.getHeading(&my, &mx, &mz);
|
||||
batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
|
||||
|
||||
double dt = (micros() - timer) / 1000000.0;
|
||||
timer = micros();
|
||||
|
||||
Serial.println(heading);
|
||||
|
||||
float tempY = atan2(-mz, my) * RAD_TO_DEG;
|
||||
|
||||
if ((tempY < -90 && heading > 90) || (tempY > 90 && heading < -90)) {
|
||||
filter.setAngle(tempY);
|
||||
heading = tempY;
|
||||
}
|
||||
|
||||
else
|
||||
|
||||
heading = filter.getAngle(tempY, gx / 131.0, dt);
|
||||
|
||||
//Serial.println(heading);
|
||||
|
||||
|
||||
while (Serial.available() > 0) {
|
||||
|
||||
// Read the next character
|
||||
|
@ -11,6 +11,6 @@
|
||||
#define BATTERY_LVL 'B'
|
||||
#define LEFT 0
|
||||
#define RIGHT 1
|
||||
#define IMU 'i'
|
||||
|
||||
#endif
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user