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

Cleanup

This commit is contained in:
Stedd 2023-10-22 12:06:14 +02:00
parent 6e266b801a
commit 212597e5e6
4 changed files with 12 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Balancebot.ino
View File

@ -2,7 +2,6 @@
#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> #include <Ps3Controller.h>
//Declare library objects //Declare library objects
@ -40,13 +39,17 @@ 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 //PS3 Controller variables
const char* _ps3Address = "18:5e:0f:92:00:6c"; const char* _ps3Address = "18:5e:0f:92:00:6c";
//UDP variables //UDP variables
uint8_t data[30 * 4]; uint8_t data[30 * 4];
void setup() { void setup() {
//Initialize serial //Initialize serial
Serial.begin(9600); Serial.begin(9600);
delay(10); delay(10);
@ -55,12 +58,8 @@ void setup() {
Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
delay(10); delay(10);
//Initialize IMU //Initialize IMU
Serial.println("Before IMU init");
IMU.init(); IMU.init();
Serial.println("After IMU init");
delay(10); delay(10);
//Initialize encoder interrupts //Initialize encoder interrupts
@ -94,17 +93,15 @@ void setup() {
} }
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
readIMU(); readIMU();
//Control motors //Control motors
motors(); motors();

3
UDP.ino
View File

@ -11,16 +11,13 @@ byte watchdog = 0;
AsyncUDP udp; AsyncUDP udp;
void UdpInit() { void UdpInit() {
//Serial.begin(115200);
ConnectToWiFi(); ConnectToWiFi();
//udp.connect(multicastIP, port);
} }
void UdpLoop() { void UdpLoop() {
udp.writeTo(data, sizeof(data), multicastIP, port); udp.writeTo(data, sizeof(data), multicastIP, port);
} }
void ConnectToWiFi() { void ConnectToWiFi() {
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password); WiFi.begin(ssid, password);

3
interruptEncoders.ino
View File

@ -93,7 +93,6 @@ void IRAM_ATTR m2_B_changed() {
} }
} }
void initEncoderInterrupt() { void initEncoderInterrupt() {
pinMode(M1_ENC_A, INPUT_PULLUP); pinMode(M1_ENC_A, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
@ -106,4 +105,4 @@ void initEncoderInterrupt() {
pinMode(M2_ENC_B, INPUT_PULLUP); pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
} }

14
motorControl.ino
View File

@ -65,16 +65,16 @@ void motors() {
IMU.init(); 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 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 //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 //Get Control Commands
rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); 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) { if (balancingOn) {
@ -157,6 +157,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
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_;
@ -171,13 +172,10 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
} else if (speedCMD_ < -saturation) { } else if (speedCMD_ < -saturation) {
windup = -saturation - speedCMD_; windup = -saturation - speedCMD_;
speedCMD_ = -saturation; speedCMD_ = -saturation;
} } else {
else {
speedCMD_ = speedCMD_; speedCMD_ = speedCMD_;
} }
//Apply speed command to PWM output //Apply speed command to PWM output
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);