mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 21:54:12 +05:30
removed imu support, added battery level correction
This commit is contained in:
parent
57dca60d0c
commit
da09baacd2
@ -1,5 +1,5 @@
|
|||||||
|
|
||||||
port: /dev/ttyUSB0
|
port: /dev/rfcomm1
|
||||||
baud: 57600
|
baud: 57600
|
||||||
timeout: 0.1
|
timeout: 0.1
|
||||||
|
|
||||||
|
@ -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.
@ -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.
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
}
|
||||||
|
@ -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;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user