Chassis.cpp

#include <Arduino.h>
#include "Chassis.h"
 
#define STOP 0
#define FORWARD 1
#define RIGHT 2
#define LEFT 4
#define BACKWARD 5
 
Chassis::Chassis(int D1, int P1, int D2, int P2) {
  _d1 = D1;
  _d2 = D2;
  _p1 = P1;
  _p2 = P2;
  _commandDuration = 0;
  _commandStart = 0;
  currentCommand = STOP;
};

void Chassis::_setMove(int command, int duration) {
  currentCommand = command;
  _commandStart = millis();
  _commandDuration = duration;
}

void Chassis::init() {
  pinMode(_d1, OUTPUT);
  pinMode(_d2, OUTPUT);
  analogWrite(_p1, 255);
  analogWrite(_p2, 255);
};

void Chassis::process() {
  int d1 = 0, d2 = 0, p1 = 0, p2 = 0;
  if (millis() - _commandStart < _commandDuration) {
    if (currentCommand == FORWARD) {
      p1 = 255;
      p2 = 255;
    };
    if (currentCommand == LEFT) {
      p1 = 0;
      p2 = 255;
    };
    if (currentCommand == RIGHT) {
      p1 = 255;
      p2 = 0;
    };
    if (currentCommand == BACKWARD) {
      d1 = 1;
      d2 = 1;
      p1 = 255;
      p2 = 255;
    };
  } else {
    currentCommand = STOP;
  }
  digitalWrite(_d1, d1);
  digitalWrite(_d2, d2);

  analogWrite(_p1, p1);
  analogWrite(_p2, p2);
};

void Chassis::idle() {
    currentCommand = STOP;
};

void Chassis::go(int duration) {
  _setMove(FORWARD, duration);
};

void Chassis::back(int duration) {
  _setMove(BACKWARD, duration);
};

void Chassis::turnLeft(int duration) {
  _setMove(LEFT, duration);
};

void Chassis::turnRight(int duration) {
  _setMove(RIGHT, duration);
};