motor_driver.ino

/***************************************************************
   Motor driver definitions
   
   Add a "#elif defined" block to this file to include support
   for a particular motor driver.  Then add the appropriate
   #define near the top of the main ROSArduinoBridge.ino file.
   
   *************************************************************/

#ifdef USE_BASE
   
#ifdef POLOLU_VNH5019
  /* Include the Pololu library */
  #include "DualVNH5019MotorShield.h"

  /* Create the motor driver object */
  DualVNH5019MotorShield drive;
  
  /* Wrap the motor driver initialization */
  void initMotorController() {
    drive.init();
  }

  /* Wrap the drive motor set speed function */
  void setMotorSpeed(int i, int spd) {
    if (i == LEFT) drive.setM1Speed(spd);
    else drive.setM2Speed(spd);
  }

  // A convenience function for setting both motor speeds
  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    setMotorSpeed(LEFT, leftSpeed);
    setMotorSpeed(RIGHT, rightSpeed);
  }
#elif defined POLOLU_MC33926
  /* Include the Pololu library */
  #include "DualMC33926MotorShield.h"

  /* Create the motor driver object */
  DualMC33926MotorShield drive;
  
  /* Wrap the motor driver initialization */
  void initMotorController() {
    drive.init();
  }

  /* Wrap the drive motor set speed function */
  void setMotorSpeed(int i, int spd) {
    if (i == LEFT) drive.setM1Speed(spd);
    else drive.setM2Speed(spd);
  }

  // A convenience function for setting both motor speeds
  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    setMotorSpeed(LEFT, leftSpeed);
    setMotorSpeed(RIGHT, rightSpeed);
  }
#elif defined L298_MOTOR_DRIVER
  void initMotorController() {
    digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
    digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
  }
  
  void setMotorSpeed(int i, int spd) {
  unsigned char reverse = 0;
  if (spd < 0)
  {
    spd = -spd;
    reverse = 1;
  }
  if (spd > 255)
    spd = 255;
  
  if (i == LEFT) { 
    if (reverse == 0) { 
      // Điều khiển động cơ trái chạy tiến
      digitalWrite(LEFT_MOTOR_FORWARD, HIGH); 
      digitalWrite(LEFT_MOTOR_BACKWARD, LOW); 
      // Điều khiển tốc độ động cơ trái bằng PWM
      ledcWrite(pwmChannel2, spd); 
    }
    else if (reverse == 1) { 
      // Điều khiển động cơ trái chạy lùi
      digitalWrite(LEFT_MOTOR_FORWARD, LOW); 
      digitalWrite(LEFT_MOTOR_BACKWARD, HIGH); 
      // Điều khiển tốc độ động cơ trái bằng PWM
      ledcWrite(pwmChannel2, spd); 
    }
  }
  else /*if (i == RIGHT) //no need for condition*/ {
    if (reverse == 0) { 
      // Điều khiển động cơ phải chạy tiến
      digitalWrite(RIGHT_MOTOR_FORWARD, HIGH); 
      digitalWrite(RIGHT_MOTOR_BACKWARD, LOW); 
      // Điều khiển tốc độ động cơ phải bằng PWM
      ledcWrite(pwmChannel1, spd); 
    }
    else if (reverse == 1) { 
      // Điều khiển động cơ phải chạy lùi
      digitalWrite(RIGHT_MOTOR_FORWARD, LOW); 
      digitalWrite(RIGHT_MOTOR_BACKWARD, HIGH); 
      // Điều khiển tốc độ động cơ phải bằng PWM
      ledcWrite(pwmChannel1, spd); 
    }
  }
}

  
  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    setMotorSpeed(LEFT, leftSpeed);
    setMotorSpeed(RIGHT, rightSpeed);
  }
#else
  #error A motor driver must be selected!
#endif

#endif