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:a0ec9092b0ac8e348fd3a36809e9f46d6962c37a
Branches Tags
Stedd:master

23 Commits
248cd4dc68 ... a0ec9092b0

Author SHA1 Message Date
Stedd a0ec9092b0 Merge branch 'UDP' 2023-10-22 12:07:46 +02:00
Stedd 212597e5e6 Cleanup 2023-10-22 12:06:14 +02:00
Stedd 6e266b801a Updated dependencies in Readme 2023-10-22 12:05:59 +02:00
Stedd 800fb1876e Fixed bug in motor windup calculation 2023-10-22 12:05:40 +02:00
Stedd 26955c1c12 Formatting 2023-10-22 02:09:46 +02:00
Stedd 1fa5f97b25 Moved velocity calculations out of balancing loop so they always can be monitored 2023-10-22 02:09:38 +02:00
Stedd 3343c2ac98 Added some more variables to UDP 2023-10-22 02:08:19 +02:00
Stedd 141a03cb84 Bot can balance by itself again 2023-10-22 02:07:22 +02:00
Stedd 3a680f84c8 Fixed a bug in LeftMotorAngular velocity calculation 2023-10-22 02:06:40 +02:00
Stedd 6a03848435 Packing data for UDP telegram 
Naive approach, but it works for now
 2023-10-22 01:18:13 +02:00
Stedd 7ab1ee525d Watchdog in the first Byte 2023-10-21 19:37:38 +02:00
Stedd 0991343a55 UDP Packets appear in wireshark and UDPlistener 2023-10-21 19:19:43 +02:00
Stedd c49c511d75 First UDP test 
Copied code from UDP example
Added calls in setup and loop
Added Wifi config
 2023-10-21 18:03:08 +02:00
Stedd b8e6e120d4 Balance Controller kind of works 
Not sure why the gains dont work anymore.
Anyways, continuing with UDP publishing.
 2023-10-21 17:51:45 +02:00
Stedd 62e07ce32a Balance controller is not working 2023-10-21 15:55:08 +02:00
Stedd c58b413392 Added board manager and libraries versions to Readme 
Had a lot of trouble compiling the project after updating packages and
libraries after launching the project after 4 years.

Hopefully having a log of used libraries makes this task easier in the
future
 2023-10-21 15:09:12 +02:00
Stedd b854a09de0 Changed index of Velocity matrix 
Compiler gave an error on "index out of bounds", have to test if turn
controller is still working after this change.

This is the first time i see this error,which leaves me a bit confused
as of i see this now.
 2023-10-21 15:07:32 +02:00
Stedd 61684e11a6 Code compiles and runs 
Getting correct pitch angle in SerialPlotter
 2023-10-21 15:00:22 +02:00
Stedd 94c0d8621b Renamed Main and placed scripts in root folder 2023-10-21 13:25:29 +02:00
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
13 changed files with 602 additions and 490 deletions
Show Stats Expand all files Collapse all files

119
Balancebot.ino Normal file
View File

@ -0,0 +1,119 @@
//Import
#include <GY_85.h>
#include <Wire.h>
#include <MatrixMath.h>
#include <Ps3Controller.h>
//Declare library objects
GY_85 IMU;
//GPIO PIN MAPPING
const byte M1_ENC_A = 32;
const byte M1_ENC_B = 33;
const byte M2_ENC_A = 34;
const byte M2_ENC_B = 35;
const byte M1_A = 16;
const byte M1_B = 17;
const byte M2_A = 18;
const byte M2_B = 19;
const int IMU_I2C_SDA = 26;
const int IMU_I2C_SCL = 27;
//Time variables
unsigned long tNow = micros();
unsigned long tLast = micros() + 13000;
int dT = 0;
float dT_s = 0.0;
//Motor variables
const int PWM_CYCLE = 12000;
const byte PWM_RES = 12;
//Encoders variables
long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast;
volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state;
//PS3 Controller variables
const char* _ps3Address = "18:5e:0f:92:00:6c";
//UDP variables
uint8_t data[30 * 4];
void setup() {
//Initialize serial
Serial.begin(9600);
delay(10);
//Initialice I2C
Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
delay(10);
//Initialize IMU
IMU.init();
delay(10);
//Initialize encoder interrupts
initEncoderInterrupt();
//Initialize encoders
m1Raw = 0;
m1RawLast = 100;
m2Raw = 0;
m2RawLast = 100;
// Initialize PWM channels
ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2);
ledcAttachPin(M2_A, 3);
ledcAttachPin(M2_B, 4);
ledcSetup(1, PWM_CYCLE, PWM_RES);
ledcSetup(2, PWM_CYCLE, PWM_RES);
ledcSetup(3, PWM_CYCLE, PWM_RES);
ledcSetup(4, PWM_CYCLE, PWM_RES);
//Initialize differential drive inverse kinematics
initMotors();
//Initialize PS3 controller connection
Ps3.begin(_ps3Address);
//Init UDP
UdpInit();
}
void loop() {
//Update time variables
tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds]
dT_s = dT * pow(10, -6); //[Cycle time in seconds]
//Get sensor data
readIMU();
//Control motors
motors();
// Plot
plot();
//Udp
UdpLoop();
//Save time for next cycle
tLast = tNow;
//Delay
delay(5);
}

65
IMU.ino Normal file
View File

@ -0,0 +1,65 @@
//CONSTANTS
const float alpha = 0.95;
const int acc_overflow_value = 65535;
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES
int ax, ay, az;
int cx, cy, cz;
int gx, gy, gz;
float gt;
float acc_pitch;
float pitch_rate;
float pitch = 0;
float pitch_prev = 0;
void readIMU() {
//Acceletometer
ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value);
az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value);
//Magnetometer
cx = IMU.compass_x(IMU.readFromCompass());
cy = IMU.compass_y(IMU.readFromCompass());
cz = IMU.compass_z(IMU.readFromCompass());
// Gyrocope
gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate
gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate
gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate
//Temperature sensor
gt = IMU.temp(IMU.readGyro());
// Pitch angle from accelerometer
acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI;
//Pitch rate from gyroscope
pitch_rate = -gx;
//Complementary filter
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = 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.
if (raw > (overflow_value_ / 2)) {
return (raw - overflow_value_);
}
else {
return raw;
}
}

67
Main/IMU.ino
View File

@ -1,67 +0,0 @@
//CONSTANTS
const float alpha = 0.95;
const int acc_overflow_value = 65535;
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES
int ax, ay, az;
int cx, cy, cz;
int gx, gy, gz;
float gt;
float acc_pitch;
float pitch_rate;
float pitch = 0;
float pitch_prev = 0;
void readIMU() {
//Acceletometer
ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value);
ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value);
az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value);
//Magnetometer
cx = IMU.compass_x( IMU.readFromCompass() );
cy = IMU.compass_y( IMU.readFromCompass() );
cz = IMU.compass_z( IMU.readFromCompass() );
// Gyrocope
gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate
gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate
gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate
//Temperature sensor
gt = IMU.temp ( IMU.readGyro() );
// Pitch angle from accelerometer
acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI;
//Pitch rate from gyroscope
pitch_rate = -gx;
//Complementary filter
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = 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.
if (raw > (overflow_value_ / 2)) {
return (raw - overflow_value_);
}
else {
return raw;
}
}

111
Main/Main.ino
View File

@ -1,111 +0,0 @@
//Import
#include <GY_85.h>
#include <Wire.h>
#include <MatrixMath.h>
//Declare library objects
GY_85 IMU;
//GPIO PIN MAPPING
const byte M1_ENC_A = 32;
const byte M1_ENC_B = 33;
const byte M2_ENC_A = 34;
const byte M2_ENC_B = 35;
const byte M1_A = 16;
const byte M1_B = 17;
const byte M2_A = 18;
const byte M2_B = 19;
const byte IMU_I2C_SCL = 26;
const byte IMU_I2C_SDA = 27;
//Time variables
unsigned long tNow = micros();
unsigned long tLast = micros() + 13000;
int dT = 0;
float dT_s = 0.0;
//Motor variables
const int PWM_CYCLE = 12000;
const byte PWM_RES = 12;
//Encoders variables
long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast;
volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state;
void setup() {
//Initialize serial
Serial.begin(57600);
delay(10);
//Initialice I2C
Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
delay(10);
//Initialize IMU
IMU.init();
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
IMU.GyroCalibrate();
delay(10);
//Initialize encoder interrupts
initInterrupt();
//Initialize encoders
m1Raw = 0;
m1RawLast = 100;
m2Raw = 0;
m2RawLast = 100;
// Initialize PWM channels
ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2);
ledcAttachPin(M2_A, 3);
ledcAttachPin(M2_B, 4);
ledcSetup(1, PWM_CYCLE, PWM_RES);
ledcSetup(2, PWM_CYCLE, PWM_RES);
ledcSetup(3, PWM_CYCLE, PWM_RES);
ledcSetup(4, PWM_CYCLE, PWM_RES);
//Initialize differential drive inverse kinematics
initMotors();
// Initialize Remote control
initRemote();
}
void loop() {
//Update time variables
tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds]
dT_s = dT * pow(10,-6); //[Cycle time in seconds]
//Get sensor data
readIMU();
//Control motors
motors();
// Plot
plot();
//Save time for next cycle
tLast = tNow;
//Delay
delay(5);
}

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);
}

178
Main/motorControl.ino
View File

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

75
Main/plot.ino
View File

@ -1,75 +0,0 @@
void plot(){
// Time
// Serial.print("dT:");
// Serial.println(dT);
// Serial.print(" ");
// Serial.print("dT_s:");
// Serial.println(dT_s);
// Serial.print(" ");
// IMU
// 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 ( "," );
// Serial.println ( gt );
// Serial.print ( " " );
// Serial.println ( acc_pitch);
// Remote control
// Serial.print("ch1:");
// Serial.print(pwm_time_ch1);
// Serial.print(" ");
// Serial.print("ch2:");
// Serial.print(pwm_time_ch2);
// Serial.print("ch1mapped:");
// Serial.print(rem_turn_speed_ref);
// Serial.print(" ");
// Serial.print("ch2mapped:");
// Serial.println(rem_speed_ref);
// Encoders
// Serial.print("m1Raw:");
// Serial.print(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
// Motors
// Serial.print("SpeedControllerOut:");
// Serial.print(SC_cont_out);
// Serial.print(" ");
// Serial.print("BalanceOLControllerOut:");
// Serial.print(OL_cont_out);
// Serial.print(" ");
// Serial.print("BalanceILControllerOut:");
// Serial.print(IL_cont_out);
// Serial.print(" ");
// Serial.print("TurnControllerOut:");
// Serial.println(TC_cont_out);
// Serial.print(" ");
// Serial.print("M1_CMD:");
// Serial.print(M1_Speed_CMD);
// Serial.print(" ");
// Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD);
// Serial.print("M1_Ang_Vel:");
// Serial.print(motor_ang_vel[0][0]);
// Serial.print(" ");
// Serial.print("M2_Ang_Vel:");
// Serial.print(motor_ang_vel[0][1]);
// Serial.print(" ");
// Serial.print("botLinVel:");
// Serial.print(vel_Matrix[0][0]);
// Serial.print(" ");
// Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]);
}

24
README.md
View File

@ -1 +1,23 @@
# Balancebot
# BalanceBot
## Arduino Board Settings
My board has this etched on it: ESP-WROOM-32
These settings allow upload:
* Board: FireBeetle-ESP32
## Dependencies
### Boards Manager
[esp32 v1.0.4](https://github.com/espressif/arduino-esp32)
#### Boards libraries
- Wire @1.0.1
- Wifi @1.0
- ESP32 Async UDP @1.0.0
### Libraries
[Ps3Controller.h @v1.1.0](https://github.com/jvpernis/esp32-ps3)
[MatrixMath.h @v1.0](https://github.com/eecharlie/MatrixMath)
[GY_85.h @commit af33c9f791618cec6ae218fe73d039276448ff4f](https://github.com/sqrtmo/GY-85-arduino/tree/master)

BIN
Spreadsheets/MotorSaturation_Bug.ods Normal file
View File

Binary file not shown.

30
UDP.ino Normal file
View File

@ -0,0 +1,30 @@
const char* ssid = "CaveBot";
const char* password = "&nHM%D2!$]Qg[VUv";
#include "WiFi.h"
#include "AsyncUDP.h"
const IPAddress multicastIP = IPAddress(239, 1, 2, 3);
int port = 1234;
byte watchdog = 0;
AsyncUDP udp;
void UdpInit() {
ConnectToWiFi();
}
void UdpLoop() {
udp.writeTo(data, sizeof(data), multicastIP, port);
}
void ConnectToWiFi() {
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
if (WiFi.waitForConnectResult() != WL_CONNECTED) {
Serial.println("WiFi Failed");
while (1) {
delay(1000);
}
}
}

38
Main/interruptEncoders.ino → interruptEncoders.ino
View File

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

192
motorControl.ino Normal file
View File

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

157
plot.ino Normal file
View File

@ -0,0 +1,157 @@
void plot() {
// Time
// Serial.print("dT:");
// Serial.println(dT);
// Serial.print(" ");
// Serial.print("dT_s:");
// Serial.println(dT_s);
// Serial.print(" ");
// IMU
// Serial.print("RollRate:");
// Serial.println(pitch_rate);
// Serial.print("Accelerometer_Pitch:");
// Serial.println(acc_pitch);
// Serial.print("Pitch:");
// Serial.println(pitch);
// Serial.print ( "," );
// Serial.println ( gt );
// Serial.print ( " " );
// Serial.println ( acc_pitch);
// Remote control
// Serial.print("ch1:");
// Serial.print(pwm_time_ch1);
// Serial.print(" ");
// Serial.print("ch2:");
// Serial.print(pwm_time_ch2);
// Serial.print("ch1mapped:");
// Serial.print(rem_turn_speed_ref);
// Serial.print(" ");
// Serial.print("ch2mapped:");
// Serial.println(rem_speed_ref);
// Encoders
// Serial.print("m1Raw:");
// Serial.println(m1Raw);
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
// // Motors
// Serial.print("SpeedControllerOut:");
// Serial.println(SC_cont_out);
// Serial.print("BalanceOLControllerOut:");
// Serial.println(OL_cont_out);
// Serial.print("BalanceILControllerOut:");
// Serial.println(IL_cont_out);
// Serial.print("TurnControllerOut:");
// Serial.println(TC_cont_out);
// Serial.print("M1_CMD:");
// Serial.println(M1_Speed_CMD);
// Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD);
// Serial.print("M1_Ang_Vel:");
// Serial.print(motor_ang_vel[0][0]);
// Serial.print(" ");
// Serial.print("M2_Ang_Vel:");
// Serial.print(motor_ang_vel[0][1]);
// Serial.print(" ");
// Serial.print("botLinVel:");
// Serial.println(vel_Matrix[0][0]);
// Serial.print(" ");
// Serial.print("botAngVel:");
// 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);
// }
int i = 0;
data[i] = watchdog++;
data[i += 1] = balancingOn << 1;
i = PackInt(i += 1, M1_Speed_CMD);
i = PackInt(i, M2_Speed_CMD);
i = PackFloat(i, acc_pitch);
i = PackFloat(i, pitch);
i = PackFloat(i, pitch_rate);
i = PackFloat(i, rem_speed_ref);
i = PackFloat(i, rem_turn_speed_ref);
i = PackFloat(i, SC_cont_out);
i = PackFloat(i, TC_cont_out);
i = PackFloat(i, OL_cont_out);
i = PackFloat(i, ref_IL);
i = PackFloat(i, act_IL);
i = PackFloat(i, error_IL);
i = PackFloat(i, IL_cont_out);
i = PackFloat(i, iError_IL);
i = PackFloat(i, IL_anti_windup);
i = PackFloat(i, speedCmd1);
i = PackFloat(i, speedCmd2);
i = PackFloat(i, vel_Matrix[0][0]);
i = PackFloat(i, vel_Matrix[1][0]);
i = PackFloat(i, motor_ang_vel[0][0]);
i = PackFloat(i, motor_ang_vel[1][0]);
i = PackLong(i, m1Raw);
i = PackLong(i, m2Raw);
}
int PackInt(int _i, int value) {
data[_i] = (value & 0x00FF);
data[_i + 1] = (value & 0xFF00) >> 8;
return _i + 2;
}
int PackLong(int _i, long value) {
data[_i] = (value & 0x000000FF);
data[_i + 1] = (value & 0x0000FF00) >> 8;
data[_i + 2] = (value & 0x00FF0000) >> 16;
data[_i + 3] = (value & 0xFF000000) >> 24;
return _i + 4;
}
union FloatToBytes {
float value;
byte bytes[4];
};
int PackFloat(int _i, float value) {
FloatToBytes converter;
converter.value = value;
for (int j = 0; j < 4; j++) {
data[_i + j] = converter.bytes[j];
}
return _i + 4;
}
// int PackFloat(int _i, float value) {
// data[_i] = (value & 0x000000FF) ;
// data[_i + 2] = (value & 0x0000FF00)>>8;
// data[_i + 3] = (value & 0x00FF0000)>>16;
// data[_i + 4] = (value & 0xFF000000)>>24;
// return _i + 4;
// }