yikestone/qt_pi
1
0
Fork 0
mirror of https://gitlab.com/yikestone/qt_pi.git synced 2025-08-03 21:54:12 +05:30
Code Issues Projects Releases Wiki Activity

minor changes

Browse Source
This commit is contained in:
yikestone 2018-08-17 23:08:54 +05:30
parent da09baacd2
commit d790520ddc
2 changed files with 3 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/libraries/Arduino/Arduino.ino
View File

@ -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);
@ -186,33 +152,10 @@ void setup() {
} }
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) {
// Read the next character // Read the next character

2
src/libraries/Arduino/commands.h
View File

@ -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