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

Expanded to handle 2 motors + cleanup of variables

This commit is contained in:
Stedd 2019-12-20 15:11:40 +01:00
parent 144b63d9f8
commit 5f57c6cb5c
3 changed files with 178 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Main/IMU.ino
View File

@ -1,5 +1,4 @@
//IMU variables //IMU variables
int ax_, ay_, az_; int ax_, ay_, az_;
int ax, ay, az; int ax, ay, az;
int cx, cy, cz; int cx, cy, cz;
@ -7,18 +6,18 @@ float gx, gy, gz, gt;
float acc_pitch; float acc_pitch;
void readIMU() { void readIMU() {
ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() ); ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() );
ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() ); ay_ = IMU.accelerometer_y( IMU.readFromAccelerometer() );
az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() ); az_ = IMU.accelerometer_z( IMU.readFromAccelerometer() );
// cx = GY85.compass_x( GY85.readFromCompass() ); // cx = IMU.compass_x( IMU.readFromCompass() );
// cy = GY85.compass_y( GY85.readFromCompass() ); // cy = IMU.compass_y( IMU.readFromCompass() );
// cz = GY85.compass_z( GY85.readFromCompass() ); // cz = IMU.compass_z( IMU.readFromCompass() );
gx = GY85.gyro_x( GY85.readGyro() ); gx = IMU.gyro_x( IMU.readGyro() );
gy = GY85.gyro_y( GY85.readGyro() ); gy = IMU.gyro_y( IMU.readGyro() );
gz = GY85.gyro_z( GY85.readGyro() ); gz = IMU.gyro_z( IMU.readGyro() );
// gt = GY85.temp ( GY85.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. //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.

175
Main/Main.ino
View File

@ -2,78 +2,148 @@
#include "GY_85.h" #include "GY_85.h"
#include <Wire.h> #include <Wire.h>
GY_85 GY85; //create the object
//Declare library objects
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;
//System variables //System variables
unsigned long tNow = micros(); unsigned long tNow = micros();
//Motor Control variables
const byte m1_in1 = 18; //Motor variables
const byte m1_in2 = 19; const int PWM_CYCLE = 12000;
//int m2_in1 = 16; const byte PWM_RESOLUTION = 12;
//int m2_in2 = 17;
//Encoders variables //Encoders variables
long int m1Raw,m1RawLast; long int m1Raw, m1RawLast;
const byte pin_m1_A = 34; long int m2Raw, m2RawLast;
const byte pin_m1_B = 35; volatile bool M1_A_state;
volatile bool A_state; volatile bool M1_B_state;
volatile bool B_state; volatile bool M2_A_state;
volatile bool M2_B_state;
//Interrupt routines //Interrupt routines
// CW = INCREASE // CW = INCREASE
// CCW = DECREASE // CCW = DECREASE
void IRAM_ATTR m1_A_changed() { void IRAM_ATTR m1_A_changed() {
A_state = digitalRead(pin_m1_A); M1_A_state = digitalRead(M1_ENC_A);
B_state = digitalRead(pin_m1_B); M1_B_state = digitalRead(M1_ENC_B);
//Rising //Rising
if (A_state == HIGH) { if (M1_A_state == HIGH) {
if (B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
else if (B_state == LOW) { else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
//Falling //Falling
else if (A_state == LOW) { else if (M1_A_state == LOW) {
if (B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
else if (B_state == LOW) { else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
} }
void IRAM_ATTR m1_B_changed() { void IRAM_ATTR m1_B_changed() {
A_state = digitalRead(pin_m1_A); M1_A_state = digitalRead(M1_ENC_A);
B_state = digitalRead(pin_m1_B); M1_B_state = digitalRead(M1_ENC_B);
//Rising //Rising
if (B_state == HIGH) { if (M1_B_state == HIGH) {
if (A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
else if (A_state == LOW) { else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
//Falling //Falling
else if (B_state == LOW) { else if (M1_B_state == LOW) {
if (A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
else if (A_state == LOW) { else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
} }
void IRAM_ATTR m2_A_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1;
}
}
//Falling
else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1;
}
}
}
void IRAM_ATTR m2_B_changed() {
M2_A_state = digitalRead(M2_ENC_A);
M2_B_state = digitalRead(M2_ENC_B);
//Rising
if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1;
}
}
//Falling
else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1;
}
}
}
void setup() { void setup() {
//Initialize serial //Initialize serial
@ -81,39 +151,44 @@ void setup() {
delay(10); delay(10);
//Initialice I2C //Initialice I2C
// Wire.begin(SCL,SDA); Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
Wire.begin(26, 27);
delay(10); delay(10);
//Initialize IMU //Initialize IMU
GY85.init(); IMU.init();
delay(10); delay(10);
//Initialize Interrupts //Initialize encoder interrupts
pinMode(pin_m1_A, INPUT_PULLUP); pinMode(M1_ENC_A, INPUT_PULLUP);
pinMode(pin_m1_B, INPUT_PULLUP); pinMode(M1_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(pin_m1_A), m1_A_changed, CHANGE); pinMode(M2_ENC_A, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(pin_m1_B), m1_B_changed, CHANGE); pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
//Initialize Encoders //Initialize encoders
m1Raw = 0; m1Raw = 0;
m1RawLast = 100; m1RawLast = 100;
m2Raw = 0;
m2RawLast = 100;
// Initialize PWM channels // Initialize PWM channels
// channels 0-15, resolution 1-16 bits, freq limits depend on resolution // channels 0-15, resolution 1-16 bits, freq limits depend on resolution
// ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits); // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
ledcAttachPin(m1_in1, 1); ledcAttachPin(M1_A, 1);
ledcAttachPin(m1_in2, 2); ledcAttachPin(M1_B, 2);
// ledcAttachPin(m2_in1, 3); ledcAttachPin(M2_A, 3);
// ledcAttachPin(m2_in2, 4); ledcAttachPin(M2_B, 4);
ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(2, 12000, 12); ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION);
// ledcSetup(3, 12000, 8); ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
// ledcSetup(4, 12000, 8); ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
Serial.println("Reference,Actual,SpeedCommand"); Serial.println("Reference,Actual,SpeedCommand");
} }
@ -121,11 +196,11 @@ void loop() {
//Update system variables //Update system variables
tNow = micros(); tNow = micros();
//// //Only print encoder value if value changed since last print //// //Only print encoder value if value changed since last print
// if (m1Raw != m1RawLast) { // if (m1Raw != m1RawLast) {
// Serial.println(m1Raw); // Serial.println(m1Raw);
// m1RawLast = m1Raw; // m1RawLast = m1Raw;
// } // }
//Sense //Sense
readIMU(); readIMU();

61
Main/motorControl.ino
View File

@ -2,39 +2,42 @@
const float wheel_diameter = 0.067708; const float wheel_diameter = 0.067708;
const float pulses_per_turn = 1320.0; const float pulses_per_turn = 1320.0;
//Tuning //Tuning
const float K = 2; const float K = 2.0;
const float I = 10; const float I = 10.0;
//Help variables //Help variables
int dEnc, dT; int dEnc, dT;
float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered; float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered;
unsigned long tLast; unsigned long tLast;
float speed_setp,referenceSpeed,actualSpeed; float speed_setp, referenceSpeed, actualSpeed;
float error, iError; float error, iError;
int speedCMD; int speedCMD;
void motorControl() { void motorControl() {
//Motor 1
speed_setp = map(acc_pitch, -90.0, 90.0, -4096, 4096);
referenceSpeed = speed_setp;
actualSpeed = lin_vel*4096;
error = referenceSpeed - actualSpeed;
iError = iError + (error*dT*pow(10,-6)*I);
speedCMD = round((error*K)+iError);
// Saturation //Speed Controller
if(speedCMD>4096){ referenceSpeed = acc_pitch * ((4096.0) / (90.0));
actualSpeed = lin_vel * 4096.0;
error = referenceSpeed - actualSpeed;
iError = iError + (error * dT * pow(10, -6) * I);
speedCMD = round((error * K) + iError);
// Speed command saturation
if (speedCMD > 4096) {
speedCMD = 4096; speedCMD = 4096;
} }
else if(speedCMD < -4096){ else if (speedCMD < -4096) {
speedCMD = -4096; speedCMD = -4096;
} }
else{ else {
speedCMD = speedCMD; speedCMD = speedCMD;
} }
//Motor 1 Control
if (speedCMD > 0) { if (speedCMD > 0) {
ledcWrite(1, 0); ledcWrite(1, 0);
ledcWrite(2, speedCMD); ledcWrite(2, speedCMD);
@ -56,27 +59,27 @@ void motorControl() {
// ledcWrite(3, 255); // ledcWrite(3, 255);
// ledcWrite(4, 255); // ledcWrite(4, 255);
//Calculate speed from encoders
dEnc = m1Raw - m1RawLast; //[pulses]
dT = tNow - tLast; //[micro sec]
dTurn = dEnc / pulses_per_turn;
ang_vel = (dTurn * 2 * PI) /(dT*0.000001); //Calculate speed from encoders
lin_vel = (dTurn * wheel_diameter * PI) /(dT*0.000001); dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle]
dT = tNow - tLast; //[Cycle time in microseconds]
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);
Serial.print(referenceSpeed); Serial.print(referenceSpeed);
Serial.print ( "," ); Serial.print ( "," );
Serial.println(actualSpeed); Serial.print(actualSpeed);
// Serial.print ( "," ); Serial.print ( "," );
// Serial.println(speedCMD); Serial.println(speedCMD);
// Serial.print ( "," ); // Serial.print ( "," );
// Serial.println(lin_vel); // Serial.println(lin_vel);
// Serial.print ( "," ); // Serial.print ( "," );
//update variables for next scan cycle
//Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
tLast = tNow; tLast = tNow;
} }