motor_control.cpp

#include "motor_control.h"
#include "pins.h"
#include "config.h"

void Motor::begin() {
  for (int i=0;i<4;i++) {
    pinMode(MOTOR_PINS[i].pwmPin, OUTPUT);
    pinMode(MOTOR_PINS[i].dirPin, OUTPUT);
    analogWrite(MOTOR_PINS[i].pwmPin, 0);
  }
}

void Motor::setWheelOutput(int idx, float normVal) {
  if (idx<0 || idx>=4) return;
  bool fwd = normVal >= 0.0f;
  int pwm = (int)(constrain(fabs(normVal), 0.0f, 1.0f) * 255.0f);
  digitalWrite(MOTOR_PINS[idx].dirPin, fwd?HIGH:LOW);
  analogWrite(MOTOR_PINS[idx].pwmPin, pwm);

  
void Motor::motorStep(unsigned long now){
  if (now - lastSpeedTick >= T_SPEED_MS)
  {
    float dt = (now - lastSpeedTick) / 1000.0f;
    lastSpeedTick = now;
    
    // compute wheel refs via SS controller only if changed
    static float wheelRefs[4] = {0.0f, 0.0f, 0.0f, 0.0f};
    if (SSctrl::refsHaveChanged()) {
      SSctrl::computeWheelRefs(vRef_linear, omegaRef, wheelRefs);
    }
    
    // compute errors per wheel and call PID update
    long cur[4];
    Enc::readCounts(cur);
    long d0 = cur[0] - prevEncCounts[0];    long d1 = cur[1] - prevEncCounts[1];
    prevEncCounts[0] = cur[0];      prevEncCounts[1] = cur[1];
    float meas0 = Enc::ticksToMeters(d0) / dt;    float meas1 = Enc::ticksToMeters(d1) / dt; // front wheels
    
    // apply pid per wheel (rear wheels use same as front for now)
    float err0 = wheelRefs[0] - meas0;    float u0 = constrain(PIDctrl::update(0, err0, dt), -MAX_PID_OUTPUT, MAX_PID_OUTPUT);
    float err1 = wheelRefs[1] - meas1;    float u1 = constrain(PIDctrl::update(1, err1, dt), -MAX_PID_OUTPUT, MAX_PID_OUTPUT);
    
    // map u to wheel outputs [-1..1]
    Motor::setWheelOutput(0, u0);
    Motor::setWheelOutput(1, u1);
    Motor::setWheelOutput(2, u0);
    Motor::setWheelOutput(3, u1);
  }
}

}