Come faccio a stabilizzare meglio il balancebot tramite il codice per far muovere i motori di pochissimo avanti ed indietro e quindi renderlo più stabile invece che farlo muovere per 20 centimetri avanti ed indietro. Vorrei rimanesse praticamente sul posto
Vi allego il codice per farvi un'idea
#include <Wire.h>
#include <Kalman.h>
#include <I2Cdev.h>
Kalman kalmanY;
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accYangle;
float gyroYangle;
float kalAngleY;
unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;
const int AIN1 = 3;
const int AIN2 = 9;
const int BIN1 = 10;
const int BIN2 = 11;
int speed;
const float Kp = 4;
const float Ki = 1;
const float Kd = 1;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
const uint8_t IMUAddress = 0x68;
const uint16_t I2C_TIMEOUT = 1000;
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop);
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop);
if (rcode) {
Serial.print(F("i2cWrite failed: "));
Serial.println(rcode);
}
return rcode;
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false);
if (rcode) {
Serial.print(F("i2cRead failed: "));
Serial.println(rcode);
return rcode;
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true);
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F("i2cRead timeout"));
return 5;
}
}
}
return 0;
}
void setup() {
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(57600);
Wire.begin();
i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false));
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100);
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
kalmanY.setAngle(accYangle);
gyroYangle = accYangle;
timer = micros();
}
void loop() {
runEvery(25)
{
dof();
if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)
{
stop();
}
else{
if (CurrentAngle < 230 && CurrentAngle > 130)
{
Pid();
Motors();
}
else
{
stop();
}
}
}
Serial.print("Accelerometer: ");
Serial.print("X = "); Serial.print(accX);
Serial.print(" | Y = "); Serial.print(accY);
Serial.print(" | Z = "); Serial.println(accZ);
Serial.print("Temperature: "); Serial.print(tempRaw/340.00+36.53); Serial.println(" C ");
Serial.print("Gyroscope: ");
Serial.print("X = "); Serial.print(gyroX);
Serial.print(" | Y = "); Serial.print(gyroY);
Serial.print(" | Z = "); Serial.println(gyroZ);
Serial.println(" ");
delay(400);
}
void Motors(){
if (speed > 0)
{
//forward
analogWrite(AIN1, speed);
analogWrite(AIN2, 0);
analogWrite(BIN1, speed);
analogWrite(BIN2, 0);
}
else
{
// backward
speed = map(speed,0,-255,0,255);
analogWrite(AIN1, 0);
analogWrite(AIN2, speed);
analogWrite(BIN1, 0);
analogWrite(BIN2, speed);
}
}
void stop()
{
analogWrite(AIN1, 0);
analogWrite(AIN2, 0);
analogWrite(BIN1, 0);
analogWrite(BIN2, 0);
}
void Pid(){
error = 180 - CurrentAngle;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
double gyroYrate = (double)gyroY/131.0;
CurrentAngle = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
}