Pi_Li_AMR/src/Arduino/ENCODERS.ino at main · madhubabugopisetti/Pi_Li_AMR · 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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
// BTS7960 dual motor — SERIAL control + QUADRATURE ENCODERS
// Commands:
//   F B G H -> direction
//   T       -> increase speed
//   Y       -> decrease speed
//   S       -> stop
// Encoder output: ENC L:<ticks> R:<ticks>

// -------- MOTOR PINS --------
#define RPWM1 5
#define LPWM1 6
#define REN1  7
#define LEN1  8

#define RPWM2 9
#define LPWM2 10
#define REN2  11
#define LEN2  12

// -------- ENCODER PINS --------
// LEFT motor encoder
#define ENC_L_A 1   // C1 (Green)
#define ENC_L_B 2   // C2 (Yellow)

// RIGHT motor encoder
#define ENC_R_A 3   // C1 (Green)
#define ENC_R_B 4   // C2 (Yellow)

volatile long ticks_L = 0;
volatile long ticks_R = 0;

unsigned long last_pub = 0;

// -------- SPEED CONTROL --------
int spd = 0;                // START SPEED = 0
const int SPD_STEP = 20;
const int SPD_MAX  = 255;

char last_dir = 'S';        // remembers last direction

// -------- ENCODER ISRs --------
void isr_left() {
  if (digitalRead(ENC_L_B))
    ticks_L++;
  else
    ticks_L--;
}

void isr_right() {
  if (digitalRead(ENC_R_B))
    ticks_R++;
  else
    ticks_R--;
}

// -------- MOTOR HELPERS --------
void stopMotors() {
  analogWrite(RPWM1, 0); analogWrite(LPWM1, 0);
  analogWrite(RPWM2, 0); analogWrite(LPWM2, 0);
}

void applyMotion() {
  switch (last_dir) {

    case 'F': // forward
      analogWrite(RPWM1, 0);   analogWrite(LPWM1, spd);
      analogWrite(RPWM2, 0);   analogWrite(LPWM2, spd);
      break;

    case 'B': // backward
      analogWrite(RPWM1, spd); analogWrite(LPWM1, 0);
      analogWrite(RPWM2, spd); analogWrite(LPWM2, 0);
      break;

    case 'G': // left
      analogWrite(RPWM1, 0);   analogWrite(LPWM1, spd);
      analogWrite(RPWM2, spd); analogWrite(LPWM2, 0);
      break;

    case 'H': // right
      analogWrite(RPWM1, spd); analogWrite(LPWM1, 0);
      analogWrite(RPWM2, 0);   analogWrite(LPWM2, spd);
      break;

    case 'S':
    default:
      stopMotors();
      break;
  }
}

// -------- SETUP --------
void setup() {
  // Motor pins
  pinMode(RPWM1, OUTPUT); pinMode(LPWM1, OUTPUT);
  pinMode(REN1, OUTPUT);  pinMode(LEN1, OUTPUT);
  pinMode(RPWM2, OUTPUT); pinMode(LPWM2, OUTPUT);
  pinMode(REN2, OUTPUT);  pinMode(LEN2, OUTPUT);

  digitalWrite(REN1, HIGH);
  digitalWrite(LEN1, HIGH);
  digitalWrite(REN2, HIGH);
  digitalWrite(LEN2, HIGH);

  // Encoder pins
  pinMode(ENC_L_A, INPUT_PULLUP);
  pinMode(ENC_L_B, INPUT_PULLUP);
  pinMode(ENC_R_A, INPUT_PULLUP);
  pinMode(ENC_R_B, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENC_L_A), isr_left, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_R_A), isr_right, RISING);

  Serial.begin(115200);
}

// -------- LOOP --------
void loop() {

  // ---- SERIAL COMMANDS ----
  if (Serial.available()) {
    char c = Serial.read();

    switch (c) {

      case 'T':   // increase speed
        spd += SPD_STEP;
        if (spd > SPD_MAX) spd = SPD_MAX;
        applyMotion();
        break;

      case 'Y':   // decrease speed
        spd -= SPD_STEP;
        if (spd < 0) spd = 0;
        applyMotion();
        break;

      case 'F':
      case 'B':
      case 'G':
      case 'H':
        last_dir = c;
        applyMotion();
        break;

      case 'S':
        last_dir = 'S';
        stopMotors();
        break;
    }
  }

  // ---- ENCODER PUBLISH @ 50 Hz ----
  if (millis() - last_pub >= 20) {
    last_pub = millis();
    Serial.print("ENC L:");
    Serial.print(ticks_L);
    Serial.print(" R:");
    Serial.println(ticks_R);
  }
}