Kits_talos_electronics/Robot.cpp at master · TalosElectronics-Lab/Kits_talos_electronics · 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
68
69
70
71
#include <Robot.h>
void Robot::compensar_motor_izquierdo(int velocidad)
{
    c_motor_izquierdo = velocidad;
}
void Robot::compensar_motor_derecho(int velocidad)
{
    c_motor_derecho = velocidad;
}
void Robot::Motores_init()
{
    Serial.begin(9600);
    pinMode(In1, OUTPUT);
    pinMode(In2, OUTPUT);
    pinMode(Pwm1, OUTPUT);

    pinMode(In3, OUTPUT);
    pinMode(In4, OUTPUT);
    pinMode(Pwm2, OUTPUT);
}
void Robot::Motores_mv(int velocidad_izquierda, int velocidad_derecha)
{
    if (velocidad_derecha == 0 && velocidad_izquierda == 0)
    {
        velocidad_izquierda=0;
        velocidad_derecha=0;
    }
    else if (velocidad_izquierda < 255 || velocidad_izquierda > 0)
    {
        velocidad_izquierda = c_motor_izquierdo + velocidad_izquierda;
    }
    else if (velocidad_derecha < 255 || velocidad_derecha > 0)
    {
        velocidad_derecha = c_motor_derecho + velocidad_derecha;
    }

    if (velocidad_izquierda > 0)
    {
        analogWrite(Pwm1, velocidad_izquierda);
        digitalWrite(In1, LOW);
        digitalWrite(In2, HIGH);
    }
    else
    {
        velocidad_izquierda = -1 * velocidad_izquierda;
        analogWrite(Pwm1, velocidad_izquierda);
        digitalWrite(In1, HIGH);
        digitalWrite(In2, LOW);
    }
    if (velocidad_derecha > 0)
    {
        analogWrite(Pwm2, velocidad_derecha);
        digitalWrite(In3, LOW);
        digitalWrite(In4, HIGH);
    }
    else
    {
        velocidad_derecha = -1 * velocidad_derecha;
        analogWrite(Pwm2, velocidad_derecha);
        digitalWrite(In3, HIGH);
        digitalWrite(In4, LOW);
    }
}

Robot::Robot()
{
}

Robot::~Robot()
{
}