zumo/encoderStuff.cpp at main · sysvion/zumo · 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
#include "encoderStuff.h"
#include <Zumo32U4.h>
#include <math.h>


void encoderStuff::resetEncoderCounts() {
  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
  expectedLeftEncoderCount = 0;
  expectedRightEncoderCount = 0;
}

// old name: calculateCorrectStrengt(int x)
int encoderStuff::NormaliseStrength(int32_t Strength) {
  if (Strength >= 0)
    return sqrt(Strength * 40);
  else
    return -sqrt(-Strength * 40);
}

void encoderStuff::correctOffset(int speedL, int speedR) {
  if ((uint8_t)(millis() - lastEncodersCheckTime) >= 50) {
    lastEncodersCheckTime = millis();

    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();

    expectedLeftEncoderCount += 0.6 * speedL;
    expectedRightEncoderCount += 0.6 * speedR;

    offsetLeftEncoderCount = expectedLeftEncoderCount - countsLeft;
    offsetRightEncoderCount = expectedRightEncoderCount - countsRight;


    if (offsetLeftEncoderCount > ALLOWED_SPEED_OFFSET ||
        offsetLeftEncoderCount < -ALLOWED_SPEED_OFFSET ) {
      correctLeft = NormaliseStrength(offsetLeftEncoderCount);
    }

    if (offsetRightEncoderCount > ALLOWED_SPEED_OFFSET ||
        offsetRightEncoderCount < -ALLOWED_SPEED_OFFSET) {
      correctRight = NormaliseStrength(offsetRightEncoderCount);
    }

  }
}

void encoderStuff::setExpectedLeftEncoderCount(int value) {
  expectedLeftEncoderCount = value;
}

void encoderStuff::setExpectedRightEncoderCount(int value) {
  expectedRightEncoderCount = value;
}

int encoderStuff::getCorrectLeft() {
  return correctLeft;
}

int encoderStuff::getCorrectRight() {
  return correctRight;
}

void encoderStuff::printCorrectionValues()
{
  char correctionValues[100];
  snprintf_P(correctionValues, sizeof(correctionValues), PSTR("%6d %6d exp L: %4d exp R: %4d"),
          countsLeft, countsRight,
          expectedLeftEncoderCount, expectedRightEncoderCount);
  Serial.println((String)correctionValues + "\toffL:" + offsetLeftEncoderCount + "\toffR:" + offsetRightEncoderCount
  + "\tcorrectLeft:" + correctLeft + "\tcorrectRight:" + correctRight
      );
}