Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Functions

This commit is contained in:
Stedd 2019-12-20 20:03:48 +01:00
parent 36254d96e6
commit 72c4f2d591
3 changed files with 126 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
Main/IMU.ino
View File

@ -33,28 +33,10 @@ void readIMU() {
// gt = IMU.temp ( IMU.readGyro() ); // 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 //Convert accelerometer
if (ax_ > 32768) { ax = convertInt(ax_, acc_overflow_value);
ax = (ax_ - acc_overflow_value); ay = convertInt(ay_, acc_overflow_value);
} az = convertInt(az_, 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_;
}
//Convert gyro //Convert gyro
@ -62,30 +44,11 @@ void readIMU() {
// gx - Pitch rate // gx - Pitch rate
// gy - Roll rate // gy - Roll rate
// gz - Yaw rate // gz - Yaw rate
// Gyro is calibrated for +-2000deg/s // Gyro is calibrated for +-2000deg/s
// Conversion is happening in GY_85.h line 48-50 // Conversion is happening in GY_85.h line 48-50
gx = convertInt(gx_, gyro_overflow_value);
if (gx_ > 2279) { gy = convertInt(gy_, gyro_overflow_value);
gx = (gx_ - gyro_overflow_value); gz = convertInt(gz_, 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_;
}
// Pitch angle from accelerometer // Pitch angle from accelerometer
@ -103,12 +66,12 @@ void readIMU() {
//Serial plotter //Serial plotter
// Serial.print ( "Pitch:" ); // Serial.print ( "Pitch:" );
// Serial.print ( pitch ); // Serial.print ( pitch );
// Serial.print (" "); // Serial.print (" ");
// Serial.print ( "Accelerometer_Pitch:" ); // Serial.print ( "Accelerometer_Pitch:" );
// Serial.print ( acc_pitch ); // Serial.print ( acc_pitch );
// Serial.print (" "); // Serial.print (" ");
// Serial.print ( "," ); // Serial.print ( "," );
// Serial.println ( gz ); // Serial.println ( gz );
// Serial.print ( "," ); // Serial.print ( "," );
@ -116,3 +79,20 @@ void readIMU() {
// Serial.print ( "," ); // Serial.print ( "," );
// Serial.println ( acc_pitch); // 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;
}

28
Main/Main.ino
View File

@ -1,7 +1,5 @@
#include <GY_85.h>
//Import //Import
#include "GY_85.h" #include <GY_85.h>
#include <Wire.h> #include <Wire.h>
@ -22,9 +20,9 @@ const byte IMU_I2C_SCL = 26;
const byte IMU_I2C_SDA = 27; const byte IMU_I2C_SDA = 27;
//System variables //Time variables
unsigned long tNow = micros(); unsigned long tNow = micros();
unsigned long tLast = micros()+13000; unsigned long tLast = micros() + 13000;
int dT = 0; int dT = 0;
@ -194,32 +192,28 @@ void setup() {
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
// Serial.println("Reference,Actual,SpeedCommand"); // Serial.println("Reference,Actual,SpeedCommand");
} }
void loop() { void loop() {
//Update system variables //Update time variables
tNow = micros(); tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds] 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(); readIMU();
//Think //Control motor
motors();
//Act //Save time
motorControl();
tLast = tNow; tLast = tNow;
//Delay //Delay
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
} }

149
Main/motorControl.ino
View File

@ -1,100 +1,99 @@
//Constants //Constants
const float wheel_diameter = 0.067708; const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
const float pulses_per_turn = 1320.0; const float WHEEL_DIAMETER = 0.067708;
const float filt_gain = 15; const float PULSES_PER_TURN = 1320.0;
//Tuning //Tuning
const float K = 3.5; const float K = 3.5;
const float I = 7.5; const float I = 7.5;
const float filter_gain = 15;
//Help variables //Help variables
int dEnc; float M1_Lin_Vel, M2_Lin_Vel;
float dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered; int M1_Speed_CMD, M2_Speed_CMD;
float dVel, diff; float M1_iError, M2_iError;
float speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered; float ref, act, error;
float error, iError;
int speedCMD; 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 //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); //Motor 1
lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001); motorControl(1, M1_Speed_CMD, MOTOR_SATURATION);
// Lowpass filter //Motor 2
// lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered)*(1-abs(diff))* dT * pow(10, -6)*filt_gain); // motorSpeed(2, M2_Speed_CMD, MOTOR_SATURATION);
lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered) * dT * pow(10, -6) * filt_gain); motorControl(2, 0, MOTOR_SATURATION);
// Serial plotter // Serial plotter
Serial.print("referenceSpeed:"); Serial.print("M1_Speed_REF:");
Serial.print(referenceSpeed * (100.0 / 4096.0)); Serial.print(ref * (100.0 / 4096.0));
Serial.print(" "); Serial.print(" ");
// Serial.print("actualSpeed:"); Serial.print("M1_Speed_ACT:");
// Serial.print(actualSpeed * (100.0 / 4096.0)); Serial.print(act * (100.0 / 4096.0));
// Serial.print(" ");
Serial.print("actualSpeedFiltered:");
Serial.print(actualSpeedFiltered * (100.0 / 4096.0));
Serial.print(" "); Serial.print(" ");
Serial.print("speedCMD:"); Serial.print("M1_Speed_CMD:");
Serial.println(speedCMD * (100.0 / 4096.0)); Serial.println(M1_Speed_CMD * (100.0 / 4096.0));
// Serial.print ( "," );
// Serial.println(lin_vel);
// Serial.print ( "," );
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; 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);
}
} }