updated controller

README.md

@@ -24,9 +24,3 @@ One that stabilizes the heading of the vehicle, and one that attempts to keep th
 
 Here is a schematic of the required circuit:   
 <img src="https://raw.githubusercontent.com/RCmags/selfBalancingRobot/main/self_balacing_robot_schem.png" width = "75%"></img>
-
-Finally, this is the intended platform:  
-<img src="https://raw.githubusercontent.com/RCmags/SelfBalancingRobot/main/images/side_view_res.jpg" width = "30%"></img>
-<img src="https://raw.githubusercontent.com/RCmags/SelfBalancingRobot/main/images/top_view_res.jpg" width = "30%"></img>  
-<img src="https://raw.githubusercontent.com/RCmags/SelfBalancingRobot/main/images/motion_disturb.gif" width = "30%"></img>
-<img src="https://raw.githubusercontent.com/RCmags/SelfBalancingRobot/main/images/motion_error.gif" width = "30%"></img>

controller/controller.ino

@@ -0,0 +1,30 @@
+#include "fusion.h"
+#include "hbridge.h"
+#include "input.h"
+#include "pid.h"
+
+void setup() {     
+  // RX signals
+  setPwmInputs();
+  delay(1000);      // allow receiver to initialize
+
+  // Calibrate imu
+  imu.setup();
+  imu.setBias();
+  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
+
+  // setup h-bridge
+  setupPulses();
+}
+
+void loop() {
+  // get attitude
+  fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az(), GAIN, SD_ACCEL );  
+
+  // get rx input
+  float input[2]; filterInputs(input);
+
+  // set motors
+  float rate[2]; PIDcontrollers(input, rate);
+  motors.setRate(rate);
+}

controller/fusion.h

@@ -0,0 +1,26 @@
+#include <basicMPU6050.h>     
+#include <imuFilter.h>
+
+// Gyro settings:
+#define         LP_FILTER   6           // Low pass filter.                    Value from 0 to 6
+#define         GYRO_SENS   2           // Gyro sensitivity.                   Value from 0 to 3
+#define         ACCEL_SENS  0           // Accelerometer sensitivity.          Value from 0 to 3
+#define         ADDRESS_A0  LOW         // I2C address from state of A0 pin.   A0 -> GND : ADDRESS_A0 = LOW
+                                        //                                     A0 -> 5v  : ADDRESS_A0 = HIGH
+// Accelerometer offset:
+constexpr int   AX_OFFSET =  552;       // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 
+constexpr int   AY_OFFSET = -241;       // These values are unlikely to be zero.
+constexpr int   AZ_OFFSET = -3185;
+
+//-- Set the template parameters:
+
+basicMPU6050<LP_FILTER,  GYRO_SENS,  ACCEL_SENS, ADDRESS_A0,
+             AX_OFFSET,  AY_OFFSET,  AZ_OFFSET
+            >imu;
+
+// =========== Settings ===========
+imuFilter fusion;
+
+#define GAIN          0.5     /* Fusion gain, value between 0 and 1 */
+#define SD_ACCEL      0.2     /* Standard deviation of acceleration. */                          
+

controller/hbridge.h

@@ -0,0 +1,90 @@
+#include <TimerOne.h>
+
+//=========== parameters ============
+
+constexpr uint32_t TIME_ON              = 4000;
+constexpr uint32_t N_STEP               = 10;
+constexpr uint32_t WAVE_PERIOD          = TIME_ON / N_STEP;
+
+constexpr uint16_t N_WAVE               = 2;
+constexpr uint8_t  WAVE_PINS[N_WAVE][2] = { {5,6}, {9,10} };
+
+//============ wave generator ==============
+
+class FrequencyPulse {
+  private:
+    uint32_t _wave_period[N_WAVE] = {TIME_ON};
+    uint32_t _dt[N_WAVE] = {0};
+    uint8_t _pin[N_WAVE] = {false};
+
+  public:
+    void setRate( float rate[] ) {
+      for( int i = 0; i < N_WAVE; i += 1 ) {
+          // turn off
+        if( rate[i] == 0 ) {
+          _pin[i] == false;
+           digitalWrite( WAVE_PINS[i][0], LOW );
+           digitalWrite( WAVE_PINS[i][1], LOW );
+
+          // modulate period
+        } else {
+          float rate_i = constrain(rate[i], -1, 1);
+
+          if( rate_i > 0 ) {
+            _pin[i] = WAVE_PINS[i][0];
+            digitalWrite( WAVE_PINS[i][1], LOW );
+
+          } else if ( rate_i < 0 ) {
+            _pin[i] = WAVE_PINS[i][1]; 
+            digitalWrite( WAVE_PINS[i][0], LOW ); 
+            rate_i = -rate_i;
+          }
+
+          _wave_period[i] = TIME_ON * (1.0/rate_i - 1);
+        }  
+      }
+    }
+
+    // interrupt function
+    void update() {
+      for( int i = 0; i < N_WAVE; i += 1 ) {
+        // infinite period
+        if( _pin[i] == false ) {
+          continue;
+        } 
+
+        // finite period
+        _dt[i] += WAVE_PERIOD;
+
+        if( _dt[i] > TIME_ON ) {
+          digitalWrite( _pin[i], LOW );
+        }
+        if( _dt[i] > _wave_period[i] ) {
+          _dt[i] = 0;
+          digitalWrite( _pin[i], HIGH ); 
+        }
+      }
+    }
+};
+
+//============ Helper functions =============
+
+FrequencyPulse motors;
+
+void timerFunc() {
+  motors.update();
+}
+
+void setupPulses() {
+  // set pins
+  for( int i = 0; i < N_WAVE; i += 1 ) {
+    for( int k = 0; k < 2; k += 1 ) {
+      int pin = WAVE_PINS[i][k];
+      pinMode(pin, OUTPUT);
+      digitalWrite(pin, LOW);
+    }
+  }
+  // timer interrupt
+  Timer1.initialize(WAVE_PERIOD);
+  Timer1.attachInterrupt(timerFunc); 
+}

controller/input.h

@@ -0,0 +1,48 @@
+#include <PulseInput.h> 
+
+//========= parameters =========
+
+constexpr int   RX_PIN1   = 2;
+constexpr int   RX_PIN2   = 3;
+
+constexpr int   BAND_MID  = 30;       // deadband at mid stick           
+constexpr int   PWM_MID   = 1550;     // PWM at mid stick
+
+constexpr float GAIN_RX[] = { -2.0 ,  // yaw 
+                             -0.01 }; // pitch
+
+//======= interrupt ========
+
+volatile unsigned int pwm_raw1, pwm_raw2;      
+
+void setPwmInputs() {
+  attachPulseInput(RX_PIN1, pwm_raw1);
+  attachPulseInput(RX_PIN2, pwm_raw2);
+}
+
+//======== filter =============
+
+float deadband(float x) {
+  if( x > BAND_MID ) {
+    return x - BAND_MID;
+  } else if ( x < -BAND_MID ) {
+    return x + BAND_MID;
+  } else {
+    return 0;
+  }
+}
+
+void filterInputs(float *input) {
+  // get update
+  input[0] = pwm_raw1;
+  input[1] = pwm_raw2;
+
+  // filter and scale
+  constexpr float SCALE = 1.0 / 500.0; 
+
+  for( int i = 0; i < 2; i += 1 ) {
+    input[i] = input[i] - PWM_MID;
+    input[i] = deadband( input[i] ); 
+    input[i] *= SCALE * GAIN_RX[i];
+  }
+}

controller/pid.h

@@ -0,0 +1,53 @@
+//=========== parameters ============
+
+//              Axis            | Description
+//------------------------------------- 
+            // pitch
+constexpr float KP_P = 5;       // fall damping
+constexpr float KI_P = 40;      // self upright - spring
+constexpr float KD_P = 0.35;    // damping
+            // position  
+constexpr float KP_X = 4;       // position - spring
+constexpr float KD_X = 2.5;     // velocity - damping
+            // yaw
+constexpr float KP_Y = 0.3;     // spring
+constexpr float KD_Y = 0.05;    // damping
+
+//========== functions =============
+
+float clamp(float x) {
+  return constrain(x, -1, 1);
+}
+
+void PIDcontrollers( float input[], float output[] ) {
+  // state [initialize at rest]
+  static float vel = 0;
+  static float pos = 0;
+
+  // pid - position
+  float dvel = vel - input[1];
+  float pid_x = KP_X*pos + KD_X*dvel;
+
+  // integrals
+  float acc = fusion.pitch() + pid_x;
+
+  float dt = fusion.timeStep();
+  vel += acc * dt;
+  vel = clamp(vel);
+
+  pos += dvel * dt;
+  pos = clamp(pos);
+
+  // pid - pitch
+  float pid_p = KP_P*acc + KI_P*vel + KD_P*imu.gy();  
+
+  // pid - yaw
+  fusion.rotateHeading( -input[0]*dt, SMALL_ANGLE);
+
+  float dy = fusion.yaw() - 0.25*input[0];
+  float pid_y = KP_Y*dy + KD_Y*imu.gz(); 
+
+  // return rates
+  output[0] = pid_p - pid_y;
+  output[1] = pid_p + pid_y;
+}