motorControl.ino

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