[UPDATE]
Here is some code that i wrote for motor control and for IMU (imu code was based on code in Joop Broking's video about MPU 6050):
Motors.hpp
Code:
#include "Arduino.h"
enum action {BACK = LOW, FORWARD = HIGH};
///////// DC_Motor ////////////////////////////////////////////////////////////
class DC_Motor
{
int __rpm, __drct;
int __ENABLE_PIN, __PHASE_PIN;
public:
DC_Motor(int, int);
void move(int, int=255);
};
///////// MotorControler //////////////////////////////////////////////////////
class MotorControler
{
DC_Motor* __LeftMotor;
DC_Motor* __RightMotor;
public:
MotorControler(DC_Motor &, DC_Motor &);
void moveForward(int=255);
void moveBack(int=255);
void spinLeft(int=255);
void spinRight(int=255);
void turnLeft(int=255);
void turnRight(int=255);
};
Motors.cpp
Code:
#include "Motors.hpp"
///////////////// DC_Motor ////////////////////////////////////////////////.
DC_Motor::DC_Motor(int E_PIN, int P_PIN) : __ENABLE_PIN(E_PIN),
__PHASE_PIN(P_PIN), __rpm(0), __drct(0)
{
pinMode(__ENABLE_PIN, OUTPUT);
pinMode(__PHASE_PIN, OUTPUT);
}
void DC_Motor::move(int direction, int pwm)
{
__rpm = pwm;
__drct = direction;
digitalWrite(__PHASE_PIN, direction);
analogWrite(__ENABLE_PIN, pwm);
}
////////////////// MotorControler ///////////////////////////////////////////
MotorControler::MotorControler(DC_Motor &left, DC_Motor &right)
{
pinMode(7, OUTPUT);
digitalWrite(7, HIGH); //enables mode 1 in DRV8335
__LeftMotor = &left;
__RightMotor = &right;
}
void MotorControler::moveForward(int pwm)
{
//__RightMotor->move(FORWARD, pwm);
__LeftMotor->move(FORWARD, pwm);
__RightMotor->move(FORWARD, pwm);
}
void MotorControler::moveBack(int pwm)
{
__LeftMotor->move(BACK, pwm);
__RightMotor->move(BACK, pwm);
}
void MotorControler::spinLeft(int pwm)
{
__LeftMotor->move(BACK, pwm);
__RightMotor->move(FORWARD, pwm);
}
void MotorControler::spinRight(int pwm)
{
__LeftMotor->move(FORWARD, pwm);
__RightMotor->move(BACK, pwm);
}
void MotorControler::turnLeft(int pwm)
{
__LeftMotor->move(BACK, 0);
__RightMotor->move(FORWARD, pwm);
}
void MotorControler::turnRight(int pwm)
{
__LeftMotor->move(FORWARD, 0);
__RightMotor->move(FORWARD, pwm);
}
Sensors.hpp
Code:
#include "Wire.h"
#include "SparkFunLSM9DS1.h"
#include "SPI.h"
class Sensor :public LSM9DS1
{
public:
int gyroX, gyroY, gyroZ;
float accX, accY, accZ, accVector;
long gyroXcal, gyroYcal, gyroZcal;
float angleRoll_G, anglePitch_G;
float angleRoll_A, anglePitch_A;
float angleRoll_Sum, anglePitch_Sum;
public:
Sensor();
void sensorCheck();
void readData();
void calibrateManual(int=2000);
};
Sensors.cpp
Code:
#include "Sensors.hpp"
Sensor::Sensor()
{
settings.device.commInterface = IMU_MODE_I2C;
settings.device.mAddress = 0x1E;
settings.device.agAddress = 0x6B;
}
void Sensor::sensorCheck()
{
if (!begin())
{
Serial.println("Failed to communicate with LSM9DS1.");
Serial.println("Double-check wiring.");
Serial.println("Default settings in this sketch will " \
"work for an out of the box LSM9DS1 " \
"Breakout, but may need to be modified " \
"if the board jumpers are.");
while (1)
;
}
else { Serial.print("Communication checked"); }
}
void Sensor::readData()
{
readGyro();
gx -= gyroXcal;
gy -= gyroYcal;
gz -= gyroZcal;
gyroX = calcGyro(gx);
gyroY = calcGyro(gy);
gyroZ = calcGyro(gz);
//-----------------------------------------
readAccel();
accX = calcAccel(ax);
accY = calcAccel(ay);
accZ = calcAccel(az) - 0.5;
accVector = sqrt((accX*accX) + (accY*accY) + (accZ*accZ));
}
void Sensor::calibrateManual(int loops)
{
for(int i = 0; i < loops; i++)
{
readGyro();
gyroXcal += gx;
gyroYcal += gy;
gyroZcal += gz;
delay(3);
}
gyroXcal /= loops;
gyroYcal /= loops;
gyroZcal /= loops;
}
and main.cpp
Code:
#include "Motors.hpp"
#include "Sensors.hpp"
const int LED = 13;
DC_Motor leftMotor(5, 6);
DC_Motor rightMotor(3, 4);
MotorControler motors(leftMotor, rightMotor);
Sensor Imu;
bool IsItSet = false;
long loop_timer;
void setup()
{
Wire.begin();
Serial.begin(38400);
pinMode(LED, OUTPUT);
digitalWrite(LED, HIGH);
Imu.sensorCheck();
Imu.calibrateManual(2000);
delay(300);
digitalWrite(LED, LOW);
loop_timer = micros();
}
void loop()
{
Imu.readData();
Imu.angleRoll_G += Imu.gyroX / 125.0;
Imu.anglePitch_G += Imu.gyroY / 125.0;
Imu.angleRoll_G += Imu.anglePitch_G * sin(Imu.gyroZ / 125 * (3.142/180));
Imu.anglePitch_G -= Imu.angleRoll_G * sin(Imu.gyroZ / 125 * (3.142/180));
//--------------------------------------------------------------------------
Imu.angleRoll_A = asin(Imu.accY/Imu.accVector)* 57.296;
Imu.anglePitch_A = asin(Imu.accX/Imu.accVector)* -57.296;
if(IsItSet)
{
Imu.angleRoll_G = Imu.angleRoll_G * 0.9 + Imu.angleRoll_A * 0.1;
Imu.anglePitch_G = Imu.anglePitch_G * 0.9 + Imu.anglePitch_A * 0.1;
}
else
{
Imu.anglePitch_G = Imu.anglePitch_A;
Imu.angleRoll_G = Imu.angleRoll_A;
IsItSet = true;
}
Serial.print(Imu.angleRoll_G);
Serial.print(", ");
Serial.println(Imu.anglePitch_G);
while(micros() - loop_timer < 8000);
loop_timer = micros();
}
Some lines in main.cpp I cann pass as Sensors method, but before I do that, I want to ask if should I change something?
It seems that that gyro has really big drift, so 0.1 ooutput is data from accelerometer. I can change sampling rate to higher, but arduino serial plotter can't keep up with it so I do not know if it actualy will improve gyro drift or not.
Now the pitch angle would be the one indicating the angle of a balancing robot.
As far as I understand now i will need kalman filter to get rid off unnecessary data, and then pass the remaining one to PID controler?
Reply With Quote
Originally Posted by Black_Stork
