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

Compare commits

base: Stedd:248cd4dc68cdcc79ccd1b48f8a5274f3a9c476a4
Branches Tags
Stedd:master
...
compare: Stedd:42034ab9e80e97d6d3bdeda9bbc373d0fce37445
Branches Tags
Stedd:master

4 Commits
248cd4dc68 ... 42034ab9e8

Author SHA1 Message Date
Stedd 42034ab9e8 this is not working 2023-10-19 20:43:01 +02:00
Stedd 72716f2abd quicksave 2023-10-19 00:26:26 +02:00
Stedd b5a53fa8c9 Cleanup 2023-10-18 22:16:53 +02:00
Stedd 23c84d2e41 Added PS3 controller 2023-10-18 21:48:18 +02:00
7 changed files with 257 additions and 269 deletions
Show Stats Expand all files Collapse all files

58
Main/IMU.ino
View File

@ -1,41 +1,45 @@
//CONSTANTS //CONSTANTS
const float alpha = 0.95; const float alpha = 0.95;
const int acc_overflow_value = 65535; const int acc_overflow_value = 65535;
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES //IMU VARIABLES
int ax, ay, az; int ax, ay, az;
int cx, cy, cz; int cx, cy, cz;
int gx, gy, gz; float gx, gy, gz;
float gt; float gt;
float acc_pitch; float acc_pitch;
float pitch_rate; float pitch_rate;
float pitch = 0; float pitch = 0;
float pitch_prev = 0; float pitch_prev = 0;
void readIMU() { void readIMU() {
// Serial.println("ReadingIMU");
//Acceletometer //Acceletometer
ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value); int* accelerometerReadings = IMU.readFromAccelerometer();
ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value); ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value);
az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value); ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value);
az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value);
//Magnetometer //Magnetometer
cx = IMU.compass_x( IMU.readFromCompass() ); int* compassReadings = IMU.readFromCompass();
cy = IMU.compass_y( IMU.readFromCompass() ); cx = IMU.compass_x(compassReadings);
cz = IMU.compass_z( IMU.readFromCompass() ); cy = IMU.compass_y(compassReadings);
cz = IMU.compass_z(compassReadings);
// Gyrocope // // Gyrocope
gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate // float* gyroReadings = IMU.readGyro();
gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate // gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate
gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate // gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate
// gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate
//Temperature sensor // //Temperature sensor
gt = IMU.temp ( IMU.readGyro() ); // gt = IMU.temp(gyroReadings);
// Pitch angle from accelerometer // Pitch angle from accelerometer
@ -47,10 +51,8 @@ void readIMU() {
//Complementary filter //Complementary filter
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = pitch; pitch_prev = pitch;
} }
@ -61,7 +63,7 @@ int convertInt(int raw, int overflow_value_) {
return (raw - overflow_value_); return (raw - overflow_value_);
} }
else { else {
return raw; return raw;
} }
} }

72
Main/Main.ino
View File

@ -2,43 +2,46 @@
#include <GY_85.h> #include <GY_85.h>
#include <Wire.h> #include <Wire.h>
#include <MatrixMath.h> #include <MatrixMath.h>
#include <Math.h>
#include <Ps3Controller.h>
//Declare library objects //Declare library objects
GY_85 IMU; GY_85 IMU;
//GPIO PIN MAPPING //GPIO PIN MAPPING
const byte M1_ENC_A = 32; const byte M1_ENC_A = 32;
const byte M1_ENC_B = 33; const byte M1_ENC_B = 33;
const byte M2_ENC_A = 34; const byte M2_ENC_A = 34;
const byte M2_ENC_B = 35; const byte M2_ENC_B = 35;
const byte M1_A = 16; const byte M1_A = 16;
const byte M1_B = 17; const byte M1_B = 17;
const byte M2_A = 18; const byte M2_A = 18;
const byte M2_B = 19; const byte M2_B = 19;
const byte IMU_I2C_SCL = 26; const int IMU_I2C_SCL = 26;
const byte IMU_I2C_SDA = 27; const int IMU_I2C_SDA = 27;
//Time 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;
float dT_s = 0.0; float dT_s = 0.0;
//Motor variables //Motor variables
const int PWM_CYCLE = 12000; const int PWM_CYCLE = 12000;
const byte PWM_RES = 12; const byte PWM_RES = 12;
//Encoders variables //Encoders variables
long int m1Raw, m1RawLast; long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast; long int m2Raw, m2RawLast;
volatile bool M1_A_state, M1_B_state; volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state; volatile bool M2_A_state, M2_B_state;
//PS3 Controller variables
const char* _ps3Address = "18:5e:0f:92:00:6c";
void setup() { void setup() {
//Initialize serial //Initialize serial
@ -46,17 +49,18 @@ void setup() {
delay(10); delay(10);
//Initialice I2C //Initialice I2C
Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); Wire.setPins(IMU_I2C_SCL, IMU_I2C_SDA);
delay(10); //delay(10);
//Initialize IMU //Initialize IMU
Serial.println("Before IMU init");
IMU.init(); IMU.init();
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values... //IMU.init();
IMU.GyroCalibrate(); Serial.println("After IMU init");
delay(10); delay(10);
//Initialize encoder interrupts //Initialize encoder interrupts
initInterrupt(); initEncoderInterrupt();
//Initialize encoders //Initialize encoders
m1Raw = 0; m1Raw = 0;
@ -78,16 +82,16 @@ void setup() {
//Initialize differential drive inverse kinematics //Initialize differential drive inverse kinematics
initMotors(); initMotors();
// Initialize Remote control //Initialize PS3 controller connection
initRemote(); Ps3.begin(_ps3Address);
} }
void loop() { void loop() {
// Serial.println("Loop");
//Update time variables //Update time variables
tNow = micros(); tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds] dT = tNow - tLast; //[Cycle time in microseconds]
dT_s = dT * pow(10,-6); //[Cycle time in seconds] dT_s = dT * pow(10, -6); //[Cycle time in seconds]
//Get sensor data //Get sensor data
@ -108,4 +112,6 @@ void loop() {
//Delay //Delay
delay(5); delay(5);
//Test
} }

35
Main/interruptEncoders.ino
View File

@ -7,8 +7,7 @@ void IRAM_ATTR m1_A_changed() {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} } else if (M1_B_state == LOW) {
else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() {
else if (M1_A_state == LOW) { else if (M1_A_state == LOW) {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} } else if (M1_B_state == LOW) {
else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} } else if (M1_A_state == LOW) {
else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() {
else if (M1_B_state == LOW) { else if (M1_B_state == LOW) {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} } else if (M1_A_state == LOW) {
else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} } else if (M2_B_state == LOW) {
else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} }
} }
@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() {
else if (M2_A_state == LOW) { else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} } else if (M2_B_state == LOW) {
else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} }
} }
@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} } else if (M2_A_state == LOW) {
else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} }
} }
@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() {
else if (M2_B_state == LOW) { else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} } else if (M2_A_state == LOW) {
else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} }
} }
} }
void initInterrupt(){ void initEncoderInterrupt() {
pinMode(M1_ENC_A, INPUT_PULLUP); pinMode(M1_ENC_A, INPUT_PULLUP);
pinMode(M1_ENC_B, INPUT_PULLUP);
pinMode(M2_ENC_A, INPUT_PULLUP);
pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
pinMode(M1_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
pinMode(M2_ENC_A, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
} }

36
Main/interruptRemote.ino
View File

@ -1,36 +0,0 @@
//Variables
const byte NO_CHANNELS = 2;
const byte CHANNEL_PINS[] = {12, 14};
volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1;
volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2;
void ch1_interrupt() {
interruptTime_ch1 = micros();
if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
}
interruptTimeLast_ch1 = interruptTime_ch1;
}
void ch2_interrupt() {
interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
}
interruptTimeLast_ch2 = interruptTime_ch2;
}
void initRemote(){
//Ch1
pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
//Ch2
pinMode(CHANNEL_PINS[1], INPUT);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE);
}

161
Main/motorControl.ino
View File

@ -1,48 +1,48 @@
//Constants //Constants
const int MOTOR_SATURATION = round(pow(2, PWM_RES)); const int MOTOR_SATURATION = round(pow(2, PWM_RES));
const float BASE_WIDTH = 0.1837; const float BASE_WIDTH = 0.1837;
const float WHEEL_DIAMETER = 0.0677; const float WHEEL_DIAMETER = 0.0677;
const float PULSES_PER_TURN = 1320.0; const float PULSES_PER_TURN = 1320.0;
const float BALANCE_POINT = 0.05; const float BALANCE_POINT = 0.05;
const float SPEED_REF = 0.00; const float SPEED_REF = 0.00;
const float TURN_SPEED_REF = 0.00; const float TURN_SPEED_REF = 0.00;
const float DEADBAND_M1_POS = 90.0; const float DEADBAND_M1_POS = 90.0;
const float DEADBAND_M1_NEG = 90.0; const float DEADBAND_M1_NEG = 90.0;
const float DEADBAND_M2_POS = 90.0; const float DEADBAND_M2_POS = 90.0;
const float DEADBAND_M2_NEG = 90.0; const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float K_SC = 18.5; //Speed controller gain const float K_SC = 18.5; //Speed controller gain
const float K_TC = 90.0; //Turn controller gain const float K_TC = 90.0; //Turn controller gain
const float K_OL = 13.0; //Outer loop balance controller gain const float K_OL = 13.0; //Outer loop balance controller gain
const float K_IL = 72.0; //Inner loop balance controller gain const float K_IL = 72.0; //Inner loop balance controller gain
const float I_IL = 80.0; //Inner loop balance controller Igain const float I_IL = 80.0; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables //Help variables
int M1_Speed_CMD, M2_Speed_CMD; int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref; float rem_speed_ref, rem_turn_speed_ref;
float SC_cont_out; float SC_cont_out;
float TC_cont_out; float TC_cont_out;
float OL_cont_out; float OL_cont_out;
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
float speedCmd1, speedCmd2;
//Matrices //Matrices
mtx_type motor_ang_vel [2][1]; mtx_type motor_ang_vel[2][1];
mtx_type vel_Matrix [2][1]; mtx_type vel_Matrix[2][1];
mtx_type inv_Kin [2][2]; mtx_type inv_Kin[2][2];
void initMotors() { void initMotors() {
// Inverse Kinematic matrix of differential drive robot // Inverse Kinematic matrix of differential drive robot
inv_Kin[0][0] = WHEEL_DIAMETER / 4; inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH; inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4; inv_Kin[0][1] = WHEEL_DIAMETER / 4;
inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH; inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH;
} }
void motors() { void motors() {
@ -56,102 +56,94 @@ void motors() {
//Calculate robot linear and angular velocity //Calculate robot linear and angular velocity
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Get Control Commands
// Remote control commands rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){ rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35);
rem_turn_speed_ref = 0;
rem_speed_ref = 0;
}
else{
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2015.0, -3.75, 3.75);
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 2005.0, -0.35, 0.35);
}
// Speed Controller // Speed Controller
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// Balance controller // Balance controller
// Outer loop // Outer loop
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
// Inner loop // Inner loop
ref_IL = OL_cont_out; ref_IL = OL_cont_out;
act_IL = pitch_rate; act_IL = pitch_rate;
error_IL = ref_IL - act_IL; error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_IL)))); iError_IL = iError_IL + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL))));
IL_cont_out = round((error_IL * K_IL) + iError_IL); IL_cont_out = round((error_IL * K_IL) + iError_IL);
//Turn controller //Turn controller
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
//Sum speed command for motors //Sum speed command for motors
M1_Speed_CMD = IL_cont_out - TC_cont_out; M1_Speed_CMD = IL_cont_out - TC_cont_out;
M2_Speed_CMD = IL_cont_out + TC_cont_out; M2_Speed_CMD = IL_cont_out + TC_cont_out;
//Sum speed command for motors //Sum speed command for motors
// M1_Speed_CMD = 0; speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0);
// M2_Speed_CMD = 0; M1_Speed_CMD = MOTOR_SATURATION * speedCmd1;
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0);
M2_Speed_CMD = MOTOR_SATURATION * speedCmd2;
motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
//Motor control //Motor control
IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); // IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); // IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
IL_anti_windup = IL_anti_windup/2; // IL_anti_windup = IL_anti_windup / 2;
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
m2RawLast = m2Raw; m2RawLast = m2Raw;
} }
float PController(float ref_, float act_, float k_){ float PController(float ref_, float act_, float k_) {
return (ref_-act_)*k_; return (ref_ - act_) * k_;
} }
float floatMap(int in, float inMin, float inMax, float outMin, float outMax){ float floatMap(int in, float inMin, float inMax, float outMin, float outMax) {
return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
} }
float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) { float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_) {
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle] 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 dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_); float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_);
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_); return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_);
} }
float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) { float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_) {
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle] 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 dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
float ang_vel_ = (dTurn_ * 2 * PI) / (dT_); float ang_vel_ = (dTurn_ * 2 * PI) / (dT_);
return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_);
} }
float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
//Returns anti windup difference //Returns anti windup difference
//Calculate channel //Calculate channel
byte ch2 = motorID * 2; byte ch2 = motorID * 2;
byte ch1 = ch2 - 1; byte ch1 = ch2 - 1;
float windup = 0; float windup = 0;
//Deadband //Deadband
if (speedCMD_ > 0 && speedCMD_ < dbPos_) { if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_; speedCMD_ = dbPos_;
} } else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
speedCMD_ = -dbNeg_; speedCMD_ = -dbNeg_;
} }
// Speed command saturation // Speed command saturation
else if (speedCMD_ > saturation) { else if (speedCMD_ > saturation) {
windup = saturation-speedCMD_; windup = saturation - speedCMD_;
speedCMD_ = saturation; speedCMD_ = saturation;
} } else if (speedCMD_ < -saturation) {
else if (speedCMD_ < -saturation) { windup = saturation - speedCMD_;
windup = saturation-speedCMD_;
speedCMD_ = -saturation; speedCMD_ = -saturation;
} }
@ -163,16 +155,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, speedCMD_); ledcWrite(ch2, speedCMD_);
} } else if (speedCMD_ < 0) {
else if (speedCMD_ < 0) {
ledcWrite(ch1, -1 * speedCMD_); ledcWrite(ch1, -1 * speedCMD_);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} } else if (speedCMD_ == 0) {
else if (speedCMD_ == 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} }
return windup; return windup;
} }

148
Main/plot.ino
View File

@ -1,75 +1,93 @@
void plot(){ void plot() {
// Time // Time
// Serial.print("dT:"); // Serial.print("dT:");
// Serial.println(dT); // Serial.println(dT);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("dT_s:"); // Serial.print("dT_s:");
// Serial.println(dT_s); // Serial.println(dT_s);
// Serial.print(" "); // Serial.print(" ");
// IMU // IMU
// Serial.print ( "Pitch:" ); Serial.print ( "Pitch:" );
// Serial.print ( pitch ); Serial.println ( 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 ( "," );
// Serial.println ( gt ); // Serial.println ( gt );
// Serial.print ( " " ); // Serial.print ( " " );
// Serial.println ( acc_pitch); // Serial.println ( acc_pitch);
// Remote control // Remote control
// Serial.print("ch1:"); // Serial.print("ch1:");
// Serial.print(pwm_time_ch1); // Serial.print(pwm_time_ch1);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("ch2:"); // Serial.print("ch2:");
// Serial.print(pwm_time_ch2); // Serial.print(pwm_time_ch2);
// Serial.print("ch1mapped:"); // Serial.print("ch1mapped:");
// Serial.print(rem_turn_speed_ref); // Serial.print(rem_turn_speed_ref);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("ch2mapped:"); // Serial.print("ch2mapped:");
// Serial.println(rem_speed_ref); // Serial.println(rem_speed_ref);
// Encoders // Encoders
// Serial.print("m1Raw:"); // Serial.print("m1Raw:");
// Serial.print(m1Raw); // Serial.print(m1Raw);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("m2Raw:"); // Serial.print("m2Raw:");
// Serial.println(m2Raw); // Serial.println(m2Raw);
// Motors // Motors
// Serial.print("SpeedControllerOut:"); // Serial.print("SpeedControllerOut:");
// Serial.print(SC_cont_out); // Serial.print(SC_cont_out);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("BalanceOLControllerOut:"); // Serial.print("BalanceOLControllerOut:");
// Serial.print(OL_cont_out); // Serial.print(OL_cont_out);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("BalanceILControllerOut:"); // Serial.print("BalanceILControllerOut:");
// Serial.print(IL_cont_out); // Serial.print(IL_cont_out);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("TurnControllerOut:"); // Serial.print("SpeedCmd1:");
// Serial.println(TC_cont_out); // Serial.println(speedCmd1);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("M1_CMD:"); // Serial.print("M1_CMD:");
// Serial.print(M1_Speed_CMD); // Serial.print(M1_Speed_CMD);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("M2_CMD:"); // Serial.print("SpeedCmd2:");
// Serial.println(M2_Speed_CMD); // Serial.println(speedCmd2);
// Serial.print(" ");
// Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD);
// Serial.print("M1_Ang_Vel:"); // Serial.print("M1_Ang_Vel:");
// Serial.print(motor_ang_vel[0][0]); // Serial.print(motor_ang_vel[0][0]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("M2_Ang_Vel:"); // Serial.print("M2_Ang_Vel:");
// Serial.print(motor_ang_vel[0][1]); // Serial.print(motor_ang_vel[0][1]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botLinVel:"); // Serial.print("botLinVel:");
// Serial.print(vel_Matrix[0][0]); // Serial.print(vel_Matrix[0][0]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botAngVel:"); // Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]); // Serial.println(vel_Matrix[1][0]);
// //PS3 Controller
// if (Ps3.isConnected()) {
// Serial.print("lx:");
// Serial.print(Ps3.data.analog.stick.lx);
// Serial.print(",");
// Serial.print("ly:");
// Serial.print(Ps3.data.analog.stick.ly);
// Serial.print(",");
// Serial.print("rx:");
// Serial.print(Ps3.data.analog.stick.rx);
// Serial.print(",");
// Serial.print("ry:");
// Serial.println(Ps3.data.analog.stick.ry);
// }
} }

16
README.md
View File

@ -1 +1,15 @@
# Balancebot # BalanceBot
## Arduino Board Settings
My board has this etched on it: ESP-WROOM-32
These settings allow upload:
* Board: FireBeetle-ESP32
## Dependencies
[Ps3Controller.h](https://github.com/jvpernis/esp32-ps3)
[MatrixMath.h](https://github.com/eecharlie/MatrixMath)
[GY_85.h](https://github.com/sqrtmo/GY-85-arduino/tree/master)