pkes/Odometry.cpp at master · dermalikmann/pkes · 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
#include "Odometry.h"
#include "Distance.h"
#include <avr/interrupt.h>

volatile bool stateA = ((PINB >> PB3) & 1);
volatile bool stateB = ((PINB >> PB4) & 1);

volatile int32_t templeft = 0;
volatile int32_t tempright = 0;
OdomData odomData = {.left=0, .right=0};

extern volatile int32_t sumOfTicks;

void initOdom() {

    DDRB &= ~(1 << PB0); //Odometry to input
    DDRB &= ~(1 << PB3);
    DDRB &= ~(1 << PB4);
    DDRB &= ~(1 << PB7);

// enable odometry Interrupts
    PCMSK0 |= (1 << PCINT3) | (1 << PCINT4);
    PCICR |= (1 << PCIE0); //Pin Change Interrupts

    sei();

};

VelocityData getVelocityData() {
    VelocityData velocityData;
    cli();
    velocityData.left = (float) templeft / 2195 / 0.075;
    velocityData.right = (float) tempright / 2195 / 0.075;
    sumOfTicks += templeft;
    templeft = 0;
    tempright = 0;
    sei();
    return velocityData;
}

ISR(PCINT0_vect){

        if (stateB != ((PINB >> PB4)&1)){ //If Change in Motor A
            stateB = !stateB;
            //Both forward criteria:

            if (stateB == ((PINB >> PB7) & 1))
                tempright++;
            else //backwards
                tempright--;
        }
// cf Bstate
        if (stateA != ((PINB >> PB3)&1)){
            stateA = !stateA;

            if (stateA == ((PINB >> PB0) & 1))
                templeft++;
            else
                templeft--;
        }
}