removed imu support, added battery level correction

This commit is contained in:
yikestone 2018-08-17 23:03:18 +05:30
parent 57dca60d0c
commit da09baacd2
7 changed files with 54 additions and 40 deletions

View File

@ -1,5 +1,5 @@
port: /dev/ttyUSB0
port: /dev/rfcomm1
baud: 57600
timeout: 0.1

View File

@ -237,7 +237,6 @@ class Arduino:
else:
if self.motors_reversed:
values[0], values[1] = -values[0], -values[1]
print values
return values
def reset_encoders(self):

Binary file not shown.

View File

@ -211,9 +211,9 @@ class BaseController:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
self.batt_lvl = self.arduino.get_batt_lvl()
self.batt_lvl = self.arduino.get_batt_lvl() / 100.0
self.batt_lvl_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900)
self.batt_lvl_pub.publish(self.batt_lvl)
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)

Binary file not shown.

View File

@ -12,14 +12,17 @@
#include "diff_controller.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU9150.h"
#include "AK8975.h"
#include "MPU6050.h"
#include "Kalman.h"
Kalman filter;
MPU9150 accelGyroMag;
AK8975 mag(0x0C);
MPU6050 accelgyro;
float batt_lvl;
int16_t gx;
float mx, my, mz;
int16_t mx, my, mz;
int dt;
@ -37,6 +40,7 @@ char argv2[16];
long arg1;
long arg2;
float heading = 0.0;
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
@ -122,10 +126,10 @@ int runCommand() {
Serial.println("OK");
break;
case BATTERY_LVL:
Serial.println(batt_lvl);
Serial.println((int)(batt_lvl * 100));
break;
case IMU:
Serial.println(heading);
Serial.println((int)(heading * 100));
break;
default:
Serial.println("Invalid Command");
@ -138,10 +142,18 @@ void setup() {
Wire.setClock(400000);
Wire.begin();
Serial.begin(BAUDRATE);
accelGyroMag.initialize();
accelgyro.initialize();
accelgyro.setI2CBypassEnabled(true);
mag.initialize();
mag.testConnection();
delay(100);
accelGyroMag.getHadjusted(&my, &mx, &mz);
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);
@ -177,13 +189,15 @@ void setup() {
uint32_t timer;
void loop() {
gx = accelGyroMag.getRotationX();
accelGyroMag.getHadjusted(&my, &mx, &mz);
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;
@ -252,4 +266,3 @@ void loop() {
}
}

View File

@ -45,10 +45,12 @@ void doPID(SetPointInfo * p) {
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
p->ITerm += (p->Ki) * Perror;
output = p->base_pwm + ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
p->PrevEnc = p->Encoder;
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
//output += p->output;
if(p->TargetTicksPerFrame > 0) output += p->base_pwm;
else if(p->TargetTicksPerFrame < 0) output -= p->base_pwm;
p->PrevEnc = p->Encoder;
if (output >= MAX_PWM)
output = MAX_PWM;