minor changes

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

View File

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

View File

@ -11,6 +11,6 @@
#define BATTERY_LVL 'B'
#define LEFT 0
#define RIGHT 1
#define IMU 'i'
#endif