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 "motor_driver.h"
|
||||||
#include "encoder_driver.h"
|
#include "encoder_driver.h"
|
||||||
#include "diff_controller.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;
|
float batt_lvl;
|
||||||
int16_t gx;
|
|
||||||
int16_t mx, my, mz;
|
|
||||||
int dt;
|
|
||||||
|
|
||||||
|
|
||||||
#define PID_RATE 30 // Hz
|
#define PID_RATE 30 // Hz
|
||||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||||
@ -39,7 +26,6 @@ char argv1[16];
|
|||||||
char argv2[16];
|
char argv2[16];
|
||||||
long arg1;
|
long arg1;
|
||||||
long arg2;
|
long arg2;
|
||||||
float heading = 0.0;
|
|
||||||
|
|
||||||
void resetCommand() {
|
void resetCommand() {
|
||||||
cmd = NULL;
|
cmd = NULL;
|
||||||
@ -128,9 +114,6 @@ int runCommand() {
|
|||||||
case BATTERY_LVL:
|
case BATTERY_LVL:
|
||||||
Serial.println((int)(batt_lvl * 100));
|
Serial.println((int)(batt_lvl * 100));
|
||||||
break;
|
break;
|
||||||
case IMU:
|
|
||||||
Serial.println((int)(heading * 100));
|
|
||||||
break;
|
|
||||||
default:
|
default:
|
||||||
Serial.println("Invalid Command");
|
Serial.println("Invalid Command");
|
||||||
break;
|
break;
|
||||||
@ -139,29 +122,12 @@ int runCommand() {
|
|||||||
|
|
||||||
/* Setup function--runs once at startup. */
|
/* Setup function--runs once at startup. */
|
||||||
void setup() {
|
void setup() {
|
||||||
Wire.setClock(400000);
|
|
||||||
Wire.begin();
|
|
||||||
Serial.begin(BAUDRATE);
|
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);
|
pinMode(A0,INPUT);
|
||||||
digitalWrite(A0,1);
|
digitalWrite(A0,1);
|
||||||
batt_lvl = analogRead(A0);
|
batt_lvl = analogRead(A0);
|
||||||
|
|
||||||
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
||||||
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
||||||
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
||||||
@ -185,33 +151,10 @@ void setup() {
|
|||||||
resetPID();
|
resetPID();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t timer;
|
|
||||||
void loop() {
|
void loop() {
|
||||||
|
|
||||||
gx = accelgyro.getRotationX();
|
|
||||||
|
|
||||||
mag.getHeading(&my, &mx, &mz);
|
|
||||||
batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
|
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) {
|
while (Serial.available() > 0) {
|
||||||
|
|
||||||
|
@ -11,6 +11,6 @@
|
|||||||
#define BATTERY_LVL 'B'
|
#define BATTERY_LVL 'B'
|
||||||
#define LEFT 0
|
#define LEFT 0
|
||||||
#define RIGHT 1
|
#define RIGHT 1
|
||||||
#define IMU 'i'
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
Loading…
x
Reference in New Issue
Block a user