combatIO/include/controlled.cpp at main · enzolfernandes/combatIO · 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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
constexpr const short minimumTriggerValue = 0;
constexpr const short maximumTriggerValue = 255;

constexpr const short minimumStickValue = -127;
constexpr const short maximumStickValue = 127;
constexpr const short deadZone = 10;
constexpr const short linearZone = deadZone + 10;
constexpr const short correction = forward * 0; // Correção para reduzir velocidade, use 0 caso não tenha diferença de velocidade entre as rodas

short x = 0, y = 0, mapXValue = 0, mapYValue = 0;

void controlledLoop()
{
        if (PS4.isConnected())
        {
                // Arma
                //! Para ESCs não reversíveis:
                weapon.write(constrain(map(PS4.R2Value(), minimumTriggerValue, maximumTriggerValue, stopped, forward), stopped, forward));

                //! Para ESCs reversíveis:
                // if (PS4.Triangle())
                // {
                //         weapon.write(forward);
                // }
                // else if (PS4.Circle())
                // {
                //         weapon.write(reverse);
                // }
                // else
                // {
                //         weapon.write(stopped);
                // }

#ifdef _DEBUG_
                Serial.println("----------");
                Serial.print("Arma: ");
                Serial.println(weapon.read());

                Serial.println();
#endif

                // Locomoção
                x = PS4.LStickX();
                mapXValue = map(x, minimumStickValue, maximumStickValue, reverse, forward);

                y = PS4.LStickY();
                mapYValue = map(y, minimumStickValue, maximumStickValue, reverse, forward);

#ifdef _DEBUG_
                Serial.print("Eixo X do Analogico: ");
                Serial.println(x);
                Serial.print("Eixo Y do Analogico: ");
                Serial.println(y);

                Serial.println();

                Serial.print("Eixo X Re-mapeado: ");
                Serial.println(mapXValue);
                Serial.print("Eixo Y Re-mapeado: ");
                Serial.println(mapYValue);

                Serial.println();
#endif

                if ((pow(x, 2) + pow(y, 2)) <= pow(deadZone, 2)) // Interior do Círculo de Zona Morta
                {
                        leftMotor.write(stopped);
                        rightMotor.write(stopped);
#ifdef _DEBUG_
                        Serial.println("Zona Morta");
#endif
                }
                else if (-linearZone < x && x < linearZone)
                {
                        leftMotor.write(constrain(mapYValue, reverse, forward) - correction);
                        rightMotor.write(constrain(mapYValue, reverse, forward) - correction);

#ifdef _DEBUG_
                        Serial.println("Zona de Movimento Linear");
                        Serial.print("Valor enviado ao motor Esquerdo: ");
                        Serial.println(leftMotor.read());
                        Serial.print("Valor enviado ao motor Direito: ");
                        Serial.println(rightMotor.read());
#endif
                }
                else if (x <= -linearZone) // Zona Esquerda
                {
                        leftMotor.write(constrain(mapYValue, stopped, reverse) - correction);
                        rightMotor.write(constrain(mapYValue, stopped, forward) - correction);

#ifdef _DEBUG_
                        Serial.println("Zona de Movimento Curvilíneo Esquerdo");
                        Serial.print("Valor enviado ao motor Esquerdo: ");
                        Serial.println(leftMotor.read());
                        Serial.print("Valor enviado ao motor Direito: ");
                        Serial.println(rightMotor.read());
#endif
                }
                else if (x >= linearZone) // Zona Direita
                {

                        leftMotor.write(constrain(mapYValue, stopped, forward) - correction);
                        rightMotor.write(constrain(mapYValue, stopped, reverse) - correction);

#ifdef _DEBUG_
                        Serial.println("Zona de Movimento Curvilíneo Direito");
                        Serial.print("Valor enviado ao motor Esquerdo: ");
                        Serial.println(leftMotor.read());
                        Serial.print("Valor enviado ao motor Direito: ");
                        Serial.println(rightMotor.read());
#endif
                }
        }
        else
        {
                leftMotor.write(stopped);
                rightMotor.write(stopped);
                weapon.write(stopped);
        }
}