Merge branch 'UDP'

Naive approach, but it works for now

Balancebot.ino

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

IMU.ino

@@ -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;
+  }
+}

Main/IMU.ino

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

Main/Main.ino

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

Main/interruptRemote.ino

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

Main/motorControl.ino

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

Main/plot.ino

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

README.md

@@ -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)

UDP.ino

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

interruptEncoders.ino

@@ -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 initEncoderInterrupt() {
-void initInterrupt(){
   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);
-}
+}

motorControl.ino

@@ -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;
+}

plot.ino

@@ -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;
+// }