Functions
This commit is contained in:
Stedd
parent
36254d96e6
commit
72c4f2d591
78
Main/IMU.ino
78
Main/IMU.ino
|
|
@ -33,28 +33,10 @@ void readIMU() {
|
|||
// gt = IMU.temp ( IMU.readGyro() );
|
||||
|
||||
|
||||
//For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
|
||||
//Convert accelerometer
|
||||
if (ax_ > 32768) {
|
||||
ax = (ax_ - acc_overflow_value);
|
||||
}
|
||||
else {
|
||||
ax = ax_;
|
||||
}
|
||||
|
||||
if (ay_ > 32768) {
|
||||
ay = (ay_ - acc_overflow_value);
|
||||
}
|
||||
else {
|
||||
ay = ay_;
|
||||
}
|
||||
|
||||
if (az_ > 32768) {
|
||||
az = (az_ - acc_overflow_value);
|
||||
}
|
||||
else {
|
||||
az = az_;
|
||||
}
|
||||
ax = convertInt(ax_, acc_overflow_value);
|
||||
ay = convertInt(ay_, acc_overflow_value);
|
||||
az = convertInt(az_, acc_overflow_value);
|
||||
|
||||
|
||||
//Convert gyro
|
||||
|
|
@ -62,30 +44,11 @@ void readIMU() {
|
|||
// gx - Pitch rate
|
||||
// gy - Roll rate
|
||||
// gz - Yaw rate
|
||||
|
||||
// Gyro is calibrated for +-2000deg/s
|
||||
// Conversion is happening in GY_85.h line 48-50
|
||||
|
||||
if (gx_ > 2279) {
|
||||
gx = (gx_ - gyro_overflow_value);
|
||||
}
|
||||
else {
|
||||
gx = gx_;
|
||||
}
|
||||
|
||||
if (gy_ > 2279) {
|
||||
gy = (gy_ - gyro_overflow_value);
|
||||
}
|
||||
else {
|
||||
gy = gy_;
|
||||
}
|
||||
|
||||
if (gz_ > 2279) {
|
||||
gz = (gz_ - gyro_overflow_value);
|
||||
}
|
||||
else {
|
||||
gz = gz_;
|
||||
}
|
||||
gx = convertInt(gx_, gyro_overflow_value);
|
||||
gy = convertInt(gy_, gyro_overflow_value);
|
||||
gz = convertInt(gz_, gyro_overflow_value);
|
||||
|
||||
|
||||
// Pitch angle from accelerometer
|
||||
|
|
@ -103,12 +66,12 @@ void readIMU() {
|
|||
|
||||
|
||||
//Serial plotter
|
||||
// Serial.print ( "Pitch:" );
|
||||
// Serial.print ( pitch );
|
||||
// Serial.print (" ");
|
||||
// Serial.print ( "Accelerometer_Pitch:" );
|
||||
// Serial.print ( acc_pitch );
|
||||
// Serial.print (" ");
|
||||
// Serial.print ( "Pitch:" );
|
||||
// Serial.print ( pitch );
|
||||
// Serial.print (" ");
|
||||
// Serial.print ( "Accelerometer_Pitch:" );
|
||||
// Serial.print ( acc_pitch );
|
||||
// Serial.print (" ");
|
||||
// Serial.print ( "," );
|
||||
// Serial.println ( gz );
|
||||
// Serial.print ( "," );
|
||||
|
|
@ -116,3 +79,20 @@ void readIMU() {
|
|||
// Serial.print ( "," );
|
||||
// Serial.println ( acc_pitch);
|
||||
}
|
||||
|
||||
|
||||
int convertInt(int raw, int overflow_value_) {
|
||||
//For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
|
||||
int conv_val;
|
||||
|
||||
if (raw > (overflow_value_ / 2)) {
|
||||
conv_val = (raw - overflow_value_);
|
||||
}
|
||||
|
||||
else {
|
||||
conv_val = raw;
|
||||
}
|
||||
|
||||
return conv_val;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
#include <GY_85.h>
|
||||
|
||||
//Import
|
||||
#include "GY_85.h"
|
||||
#include <GY_85.h>
|
||||
#include <Wire.h>
|
||||
|
||||
|
||||
|
|
@ -10,21 +8,21 @@ GY_85 IMU;
|
|||
|
||||
|
||||
//GPIO PIN MAPPING
|
||||
const byte M1_ENC_A = 34;
|
||||
const byte M1_ENC_B = 35;
|
||||
const byte M2_ENC_A = 32;
|
||||
const byte M2_ENC_B = 33;
|
||||
const byte M1_A = 18;
|
||||
const byte M1_B = 19;
|
||||
const byte M2_A = 16;
|
||||
const byte M2_B = 17;
|
||||
const byte IMU_I2C_SCL = 26;
|
||||
const byte IMU_I2C_SDA = 27;
|
||||
const byte M1_ENC_A = 34;
|
||||
const byte M1_ENC_B = 35;
|
||||
const byte M2_ENC_A = 32;
|
||||
const byte M2_ENC_B = 33;
|
||||
const byte M1_A = 18;
|
||||
const byte M1_B = 19;
|
||||
const byte M2_A = 16;
|
||||
const byte M2_B = 17;
|
||||
const byte IMU_I2C_SCL = 26;
|
||||
const byte IMU_I2C_SDA = 27;
|
||||
|
||||
|
||||
//System variables
|
||||
//Time variables
|
||||
unsigned long tNow = micros();
|
||||
unsigned long tLast = micros()+13000;
|
||||
unsigned long tLast = micros() + 13000;
|
||||
int dT = 0;
|
||||
|
||||
|
||||
|
|
@ -194,32 +192,28 @@ void setup() {
|
|||
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
|
||||
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
|
||||
|
||||
// Serial.println("Reference,Actual,SpeedCommand");
|
||||
// Serial.println("Reference,Actual,SpeedCommand");
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//Update system variables
|
||||
//Update time variables
|
||||
tNow = micros();
|
||||
dT = tNow - tLast; //[Cycle time in microseconds]
|
||||
|
||||
//// //Only print encoder value if value changed since last print
|
||||
// if (m1Raw != m1RawLast) {
|
||||
// Serial.println(m1Raw);
|
||||
// m1RawLast = m1Raw;
|
||||
// }
|
||||
|
||||
//Sense
|
||||
//Get sensor data
|
||||
readIMU();
|
||||
|
||||
|
||||
//Think
|
||||
//Control motor
|
||||
motors();
|
||||
|
||||
|
||||
//Act
|
||||
motorControl();
|
||||
|
||||
//Save time
|
||||
tLast = tNow;
|
||||
|
||||
|
||||
//Delay
|
||||
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,100 +1,99 @@
|
|||
//Constants
|
||||
const float wheel_diameter = 0.067708;
|
||||
const float pulses_per_turn = 1320.0;
|
||||
const float filt_gain = 15;
|
||||
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
|
||||
const float WHEEL_DIAMETER = 0.067708;
|
||||
const float PULSES_PER_TURN = 1320.0;
|
||||
|
||||
//Tuning
|
||||
const float K = 3.5;
|
||||
const float I = 7.5;
|
||||
const float K = 3.5;
|
||||
const float I = 7.5;
|
||||
const float filter_gain = 15;
|
||||
|
||||
//Help variables
|
||||
int dEnc;
|
||||
float dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered;
|
||||
float dVel, diff;
|
||||
float M1_Lin_Vel, M2_Lin_Vel;
|
||||
int M1_Speed_CMD, M2_Speed_CMD;
|
||||
float M1_iError, M2_iError;
|
||||
|
||||
float speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered;
|
||||
float error, iError;
|
||||
int speedCMD;
|
||||
float ref, act, error;
|
||||
|
||||
void motors() {
|
||||
|
||||
//Controllers
|
||||
ref = pitch * ((4096.0) / (90.0));
|
||||
act = M1_Lin_Vel * 4096.0;
|
||||
error = ref - act;
|
||||
M1_iError = M1_iError + (error * dT * pow(10, -6) * I);
|
||||
M1_Speed_CMD = round((error * K) + M1_iError);
|
||||
|
||||
// ref = pitch * ((4096.0) / (90.0));
|
||||
// act = M2_Lin_Vel * 4096.0;
|
||||
// error = ref - act;
|
||||
// M2_iError = M2_iError + (error * dT * pow(10, -6) * I);
|
||||
// M2_Speed_CMD = round((error * K) + M2_iError);
|
||||
|
||||
|
||||
void motorControl() {
|
||||
|
||||
//Speed Controller
|
||||
referenceSpeed = pitch * ((4096.0) / (90.0));
|
||||
actualSpeed = lin_vel * 4096.0;
|
||||
actualSpeedFiltered = lin_vel_filtered * 4096.0;
|
||||
error = referenceSpeed - actualSpeedFiltered;
|
||||
iError = iError + (error * dT * pow(10, -6) * I);
|
||||
speedCMD = round((error * K) + iError);
|
||||
|
||||
|
||||
// Speed command saturation
|
||||
if (speedCMD > 4096) {
|
||||
speedCMD = 4096;
|
||||
}
|
||||
else if (speedCMD < -4096) {
|
||||
speedCMD = -4096;
|
||||
}
|
||||
else {
|
||||
speedCMD = speedCMD;
|
||||
}
|
||||
|
||||
|
||||
//Motor 1 Control
|
||||
if (speedCMD > 0) {
|
||||
ledcWrite(1, 0);
|
||||
ledcWrite(2, speedCMD);
|
||||
}
|
||||
else if (speedCMD < 0) {
|
||||
ledcWrite(1, -1 * speedCMD);
|
||||
ledcWrite(2, 0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//Motor 2
|
||||
// ledcWrite(3, 255);
|
||||
// ledcWrite(4, 255);
|
||||
|
||||
|
||||
//No speed command
|
||||
// ledcWrite(1, 0);
|
||||
// ledcWrite(2, 0);
|
||||
ledcWrite(3, 0);
|
||||
ledcWrite(4, 0);
|
||||
|
||||
|
||||
//Calculate speed from encoders
|
||||
dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle]
|
||||
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
|
||||
// M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
|
||||
|
||||
dTurn = dEnc / pulses_per_turn; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
|
||||
ang_vel = (dTurn * 2 * PI) / (dT * 0.000001);
|
||||
lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001);
|
||||
//Motor 1
|
||||
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION);
|
||||
|
||||
// Lowpass filter
|
||||
// lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered)*(1-abs(diff))* dT * pow(10, -6)*filt_gain);
|
||||
lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered) * dT * pow(10, -6) * filt_gain);
|
||||
//Motor 2
|
||||
// motorSpeed(2, M2_Speed_CMD, MOTOR_SATURATION);
|
||||
motorControl(2, 0, MOTOR_SATURATION);
|
||||
|
||||
|
||||
// Serial plotter
|
||||
Serial.print("referenceSpeed:");
|
||||
Serial.print(referenceSpeed * (100.0 / 4096.0));
|
||||
Serial.print("M1_Speed_REF:");
|
||||
Serial.print(ref * (100.0 / 4096.0));
|
||||
Serial.print(" ");
|
||||
// Serial.print("actualSpeed:");
|
||||
// Serial.print(actualSpeed * (100.0 / 4096.0));
|
||||
// Serial.print(" ");
|
||||
Serial.print("actualSpeedFiltered:");
|
||||
Serial.print(actualSpeedFiltered * (100.0 / 4096.0));
|
||||
Serial.print("M1_Speed_ACT:");
|
||||
Serial.print(act * (100.0 / 4096.0));
|
||||
Serial.print(" ");
|
||||
Serial.print("speedCMD:");
|
||||
Serial.println(speedCMD * (100.0 / 4096.0));
|
||||
// Serial.print ( "," );
|
||||
// Serial.println(lin_vel);
|
||||
// Serial.print ( "," );
|
||||
Serial.print("M1_Speed_CMD:");
|
||||
Serial.println(M1_Speed_CMD * (100.0 / 4096.0));
|
||||
|
||||
|
||||
//Update variables for next scan cycle
|
||||
m1RawLast = m1Raw;
|
||||
|
||||
|
||||
m2RawLast = m2Raw;
|
||||
}
|
||||
|
||||
float encoderReader(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, int dT_, float filt_gain_ ) {
|
||||
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
|
||||
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001);
|
||||
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_);
|
||||
}
|
||||
|
||||
void motorControl(byte motorID, int speedCMD_, int saturation) {
|
||||
//Calculate channel
|
||||
byte ch1 = motorID * 2 - 1;
|
||||
byte ch2 = motorID * 2;
|
||||
|
||||
|
||||
// Speed command saturation
|
||||
if (speedCMD_ > saturation) {
|
||||
speedCMD_ = saturation;
|
||||
}
|
||||
else if (speedCMD_ < -saturation) {
|
||||
speedCMD_ = -saturation;
|
||||
}
|
||||
else {
|
||||
speedCMD_ = speedCMD_;
|
||||
}
|
||||
|
||||
|
||||
//Motor Control
|
||||
if (speedCMD_ > 0) {
|
||||
ledcWrite(ch1, 0);
|
||||
ledcWrite(ch2, speedCMD_);
|
||||
}
|
||||
else if (speedCMD_ < 0) {
|
||||
ledcWrite(ch1, -1 * speedCMD_);
|
||||
ledcWrite(ch2, 0);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue