MatrixMini/src/MiniDC.cpp at master · Matrix-Robotics/MatrixMini · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#include "MiniDC.h"


void MiniDC::begin(int ver, int pin){
  _ver = ver;
  _pin = pin;
}

// -100 to 100
void MiniDC::set(int speed) {
  if(speed < 0){
    _mode = BACKWARD;
    speed *= -1;
  }
  else{
    _mode = FORWARD;
  }

  if(_ver < 3){
     _speed = map(speed, 0, 100, 0, 4095);
    setMode_PCA9685();
  }
  else{
     _speed = map(speed, 0, 100, 0, 255);
    setMode_PCA9633();
  }
}

void MiniDC::setMode_PCA9633() {
  if(_mode == FORWARD){
    setPWM_PCA9633(_pin, 0, _speed);
  }
  else{
    setPWM_PCA9633(_pin, _speed, 0);
  }
}

void MiniDC::setMode_PCA9685() {
  if(_pin == 5){
    if(_mode == FORWARD){
      setPWM_PCA9685(_pin-2, 4095, 0, _speed); // IN1, IN2, PWM
    }
    else{
      setPWM_PCA9685(_pin-2, 0, 4095, _speed); // IN1, IN2, PWM
    }
  }
  else{
    if(_mode == FORWARD){
      setPWM_PCA9685(_pin, _speed, 0, 4095); // PWM, IN2, IN1
    }
    else{
      setPWM_PCA9685(_pin, _speed, 4095, 0); // PWM, IN2, IN1
    }
  }
}