WPIBotOnAWire-Arduino/src/radio-ROS.cpp at main · WPIBotOnAWire/WPIBotOnAWire-Arduino · 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
#include "radio-ROS.h"
#include "ESmotor.h"
#include <std_msgs/Bool.h>

#include "robot.h"

#define RADIO_OVERRIDE_PIN  12      // slot 5 of the radio receiver
#define RADIO_SPEED_PIN     13      // slot 1 of the radio receiver

std_msgs::Bool radioOverrideMsg;
ros::Publisher pubRadioOverride("/radio/override", &radioOverrideMsg);

RadioPulse radioOverride(RADIO_OVERRIDE_PIN);
void ISR_RADIO_OVERRIDE(void) {radioOverride.radioISR();}

RadioPulse radioSpeed(RADIO_SPEED_PIN);
void ISR_RADIO_SPEED(void) {radioSpeed.radioISR();}

void setupRadio(ros::NodeHandle& nh)
{
    DEBUG_SERIAL.println("setupRadio");
    nh.advertise(pubRadioOverride);

    radioOverride.Init(ISR_RADIO_OVERRIDE);
    radioSpeed.Init(ISR_RADIO_SPEED);
}

const uint8_t minOverrideCount = 5; // to avoid spikes, which are annoying
int8_t overrideCount = 0;
bool override = false;

int8_t directionCount = 0;

void processRadio(void)
{
    uint32_t overridePulseLength = 0;
    if(radioOverride.GetPulseWidth(overridePulseLength))
    {
        //DEBUG_SERIAL.println(overridePulseLength);
        bool prev_override = override; // I don't like this here -- should go at end?

        if(overridePulseLength >= 2010)
        {
            DEBUG_SERIAL.print("Spike: ");
            DEBUG_SERIAL.println(overridePulseLength);
        }

        /**
         * This bit of code makes it so we have to get N overrides in a row to switch over.
         * Been having issues with spikes.
        */
        if(overridePulseLength > 1900 && overridePulseLength < 2010)
        {
            if(++overrideCount >= minOverrideCount)
            {
                overrideCount = minOverrideCount; // cap it so we don't roll over
                override = true;
            }
        }
        else if (overridePulseLength < 1200 && overridePulseLength > 900)
        {
            if(--overrideCount <= 0)
            {
                override = false;
                overrideCount = 0;
            }
        }

        radioOverrideMsg.data = override;
        pubRadioOverride.publish(&radioOverrideMsg);

        // if(override && !prev_override)
        // {
        //     DEBUG_SERIAL.print("Override: ");
        //     DEBUG_SERIAL.print('\t');
        //     DEBUG_SERIAL.print(overridePulseLength);
        //     DEBUG_SERIAL.print('\n');
        // }

        if(prev_override && !override) robot.SetAuto();
        if(override && !prev_override) robot.SetTeleop();
    }

    uint32_t radioSpeedPulse = 0;
    if(radioSpeed.GetPulseWidth(radioSpeedPulse))
    {
        //DEBUG_SERIAL.println(radioSpeedPulse);
        /**
         * If we're in override mode, use the speed pulse to directly command the motors.
        */
        if(override)
        {
            if(radioSpeedPulse > 2100 || radioSpeedPulse < 900) {} //ignore the spikes
            esMotor.SetPulseDirect(radioSpeedPulse);
        }

        /**
         * If we're not in override mode, use the pulses to command the patrol direction.
        */
        else
        {
            if(radioSpeedPulse < 1200) directionCount--;
            else if(radioSpeedPulse > 1800) directionCount++;
            else directionCount = 0; //but don't reset direction; just avoid spikes

            if(directionCount < -5)
            {
                directionCount = -5;
                robot.SetDirection(Robot::DIR_REV);
            }

            else if(directionCount > 5)
            {
                directionCount = 5;
                robot.SetDirection(Robot::DIR_FWD);
            }
        }
    }
}