Motor/motor.cpp at master · Compotes/Motor · 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
57
58
59
60
61
62
63
64
65
66
67
#include "Arduino.h"
#include "motor.h"

Motor::Motor() {
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);

    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);

    pinMode(6, OUTPUT);
    pinMode(7, OUTPUT);

    pinMode(8, OUTPUT);
    pinMode(9, OUTPUT);
}

void Motor::stop(int motor, int motor2, int motor3, int motor4) {
    int arr[] = {motor, motor2, motor3, motor4};
    for (int i = 0; i < sizeof(arr) / sizeof(*arr); i++) {
        analogWrite(arr[i] * 2, 0);
        analogWrite(arr[i] * 2 + 1, 0);
    }
}

void Motor::go_all(int speeds[], int directions[]) {
    for (int i = 1; i <= MOTOR_NUM ; i++)
        go(i, directions[i - 1], speeds[i - 1]);
}

void Motor::go(int motor, int dir, int speed) {
    if (speed < 0)
        speed = 0;

    if (dir == 1) {
        analogWrite(motor * 2, speed);
        analogWrite(motor * 2 + 1, 0);
    } else {
        analogWrite(motor * 2, 0);
        analogWrite(motor * 2 + 1, speed);
    }
}

void Motor::go_pair(int motor1, int motor2, int dir, int speed) {
    if (dir == 1) {
            go(motor1, 1, speed);
            go(motor2, 0, speed);
    } else {
        go(motor1, 0, speed);
        go(motor2, 1, speed);
    }
}

void Motor::rotate(int dir, int speed) {
    if (speed < 0)
        speed = 0;

    for (int i = 1; i < 5; i++) {
        if (dir == 1) {
            analogWrite(i * 2, speed);
            analogWrite(i * 2 + 1, 0);
        } else {
            analogWrite(i * 2, 0);
            analogWrite(i * 2 + 1, speed);
        }
    }
}