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 baud: 57600
timeout: 0.1 timeout: 0.1

View File

@ -237,7 +237,6 @@ class Arduino:
else: else:
if self.motors_reversed: if self.motors_reversed:
values[0], values[1] = -values[0], -values[1] values[0], values[1] = -values[0], -values[1]
print values
return values return values
def reset_encoders(self): 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.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta 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): def stop(self):
self.stopped = True self.stopped = True
self.arduino.drive(0, 0) self.arduino.drive(0, 0)

Binary file not shown.

View File

@ -12,14 +12,17 @@
#include "diff_controller.h" #include "diff_controller.h"
#include <Wire.h> #include <Wire.h>
#include "I2Cdev.h" #include "I2Cdev.h"
#include "MPU9150.h" #include "AK8975.h"
#include "MPU6050.h"
#include "Kalman.h" #include "Kalman.h"
Kalman filter; Kalman filter;
MPU9150 accelGyroMag; AK8975 mag(0x0C);
MPU6050 accelgyro;
float batt_lvl; float batt_lvl;
int16_t gx; int16_t gx;
float mx, my, mz; int16_t mx, my, mz;
int dt; int dt;
@ -37,6 +40,7 @@ char argv2[16];
long arg1; long arg1;
long arg2; long arg2;
float heading = 0.0; float heading = 0.0;
void resetCommand() { void resetCommand() {
cmd = NULL; cmd = NULL;
memset(argv1, 0, sizeof(argv1)); memset(argv1, 0, sizeof(argv1));
@ -54,12 +58,12 @@ int runCommand() {
int pid_args[4]; int pid_args[4];
arg1 = atoi(argv1); arg1 = atoi(argv1);
arg2 = atoi(argv2); arg2 = atoi(argv2);
switch(cmd) { switch(cmd) {
case GET_BAUDRATE: case GET_BAUDRATE:
Serial.println(BAUDRATE); Serial.println(BAUDRATE);
break; break;
case READ_ENCODERS: case READ_ENCODERS:
Serial.print(readEncoder(LEFT)); Serial.print(readEncoder(LEFT));
Serial.print(" "); Serial.print(" ");
@ -85,7 +89,7 @@ int runCommand() {
rightPID.TargetTicksPerFrame = arg2; rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK"); Serial.println("OK");
break; break;
case UPDATE_PID_L: case UPDATE_PID_L:
while ((str = strtok_r(p, ":", &p)) != '\0') { while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str); pid_args[i] = atoi(str);
@ -122,10 +126,10 @@ int runCommand() {
Serial.println("OK"); Serial.println("OK");
break; break;
case BATTERY_LVL: case BATTERY_LVL:
Serial.println(batt_lvl); Serial.println((int)(batt_lvl * 100));
break; break;
case IMU: case IMU:
Serial.println(heading); Serial.println((int)(heading * 100));
break; break;
default: default:
Serial.println("Invalid Command"); Serial.println("Invalid Command");
@ -136,17 +140,25 @@ int runCommand() {
/* Setup function--runs once at startup. */ /* Setup function--runs once at startup. */
void setup() { void setup() {
Wire.setClock(400000); Wire.setClock(400000);
Wire.begin(); Wire.begin();
Serial.begin(BAUDRATE); Serial.begin(BAUDRATE);
accelGyroMag.initialize();
accelgyro.initialize();
accelgyro.setI2CBypassEnabled(true);
mag.initialize();
mag.testConnection();
delay(100); 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; heading = atan2(-mz, my) * RAD_TO_DEG;
filter.setQangle(0.0001); filter.setQangle(0.0001);
filter.setQbias(0.003); filter.setQbias(0.003);
filter.setRmeasure(0.05); filter.setRmeasure(0.05);
filter.setAngle(heading); filter.setAngle(heading);
pinMode(A0,INPUT); pinMode(A0,INPUT);
digitalWrite(A0,1); digitalWrite(A0,1);
batt_lvl = analogRead(A0); batt_lvl = analogRead(A0);
@ -154,53 +166,55 @@ void setup() {
DDRC &= ~(1<<LEFT_ENC_PIN_B); DDRC &= ~(1<<LEFT_ENC_PIN_B);
DDRD &= ~(1<<RIGHT_ENC_PIN_A); DDRD &= ~(1<<RIGHT_ENC_PIN_A);
DDRD &= ~(1<<RIGHT_ENC_PIN_B); DDRD &= ~(1<<RIGHT_ENC_PIN_B);
//enable pull up resistors //enable pull up resistors
PORTC |= (1<<LEFT_ENC_PIN_A); PORTC |= (1<<LEFT_ENC_PIN_A);
PORTC |= (1<<LEFT_ENC_PIN_B); PORTC |= (1<<LEFT_ENC_PIN_B);
PORTD |= (1<<RIGHT_ENC_PIN_A); PORTD |= (1<<RIGHT_ENC_PIN_A);
PORTD |= (1<<RIGHT_ENC_PIN_B); PORTD |= (1<<RIGHT_ENC_PIN_B);
// tell pin change mask to listen to left encoder pins // tell pin change mask to listen to left encoder pins
PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B); PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins // tell pin change mask to listen to right encoder pins
PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B); PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2); PCICR |= (1 << PCIE1) | (1 << PCIE2);
initMotorController(); initMotorController();
resetPID(); resetPID();
}
}
uint32_t timer; uint32_t timer;
void loop() { void loop() {
gx = accelGyroMag.getRotationX(); gx = accelgyro.getRotationX();
accelGyroMag.getHadjusted(&my, &mx, &mz);
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; double dt = (micros() - timer) / 1000000.0;
timer = micros(); timer = micros();
Serial.println(heading);
float tempY = atan2(-mz, my) * RAD_TO_DEG; float tempY = atan2(-mz, my) * RAD_TO_DEG;
if ((tempY < -90 && heading > 90) || (tempY > 90 && heading < -90)) { if ((tempY < -90 && heading > 90) || (tempY > 90 && heading < -90)) {
filter.setAngle(tempY); filter.setAngle(tempY);
heading = tempY; heading = tempY;
} }
else else
heading = filter.getAngle(tempY, gx / 131.0, dt); heading = filter.getAngle(tempY, gx / 131.0, dt);
//Serial.println(heading); //Serial.println(heading);
while (Serial.available() > 0) { while (Serial.available() > 0) {
// Read the next character // Read the next character
chr = Serial.read(); chr = Serial.read();
@ -238,18 +252,17 @@ void loop() {
} }
} }
} }
if (millis() > nextPID) { if (millis() > nextPID) {
updatePID(); updatePID();
nextPID += PID_INTERVAL; nextPID += PID_INTERVAL;
} }
// Check to see if we have exceeded the auto-stop interval // Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {; if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0); setMotorSpeeds(0, 0);
moving = 0; moving = 0;
resetPID(); resetPID();
} }
}
}

View File

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