Skip to content
This repository was archived by the owner on Jul 25, 2022. It is now read-only.

Commit 0a5cae7

Browse files
dhebbekerdhebbeker
committed
Back to static Drive class template.
Simplified ISR for odometry.
1 parent 5a029ad commit 0a5cae7

File tree

5 files changed

+51
-98
lines changed

5 files changed

+51
-98
lines changed

‎robot-control-src/Drives.cpp

Lines changed: 12 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -5,25 +5,15 @@
55

66
#undef round //see https://github.com/esp8266/Arduino/issues/5787#issuecomment-465852231
77

8-
template<typename MOTORCONTROL, typename DIRECTIONPIN, typename ODOPIN>
9-
static drives::Drive<MOTORCONTROL, DIRECTIONPIN, ODOPIN> makeDrive(MOTORCONTROL& motorControlPin, DIRECTIONPIN& directionPin, ODOPIN& odoPin)
10-
{
11-
return drives::Drive<MOTORCONTROL, DIRECTIONPIN, ODOPIN>(motorControlPin, directionPin, odoPin);
12-
}
13-
148
namespace drives
159
{
16-
static auto l = makeDrive(board::leftMotor, board::leftBackwards, board::leftOdoSignal);
17-
static auto r = makeDrive(board::rightMotor, board::rightBackwards, board::rightOdoSignal);
1810

19-
DriveInterface& leftDrive = l;
20-
DriveInterface& rightDrive = r;
21-
22-
IRAM_ATTR void odometryCounter()
23-
{
24-
leftDrive.evaluateInterval();
25-
rightDrive.evaluateInterval();
26-
}
11+
template<typename MOTORCONTROL, MOTORCONTROL &motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN &directionPin, typename ODOPIN, ODOPIN &odoPin>
12+
std::uint16_t volatile Drive<MOTORCONTROL, motorControlpin, DIRECTIONPIN, directionPin, ODOPIN, odoPin>::counter = 0;
13+
template<typename MOTORCONTROL, MOTORCONTROL &motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN &directionPin, typename ODOPIN, ODOPIN &odoPin>
14+
std::uint16_t Drive<MOTORCONTROL, motorControlpin, DIRECTIONPIN, directionPin, ODOPIN, odoPin>::target = 0;
15+
template<typename MOTORCONTROL, MOTORCONTROL &motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN &directionPin, typename ODOPIN, ODOPIN &odoPin>
16+
bool Drive<MOTORCONTROL, motorControlpin, DIRECTIONPIN, directionPin, ODOPIN, odoPin>::isIdle = true;
2717

2818
/**
2919
* The right drive tends to be faster than the left. In order to compensate a factor
@@ -57,14 +47,14 @@ static Amplitude calcRightSpeed(const Amplitude leftSpeed)
5747

5848
void rotateCounter(const Counter steps, const Amplitude amplitude, bool const clockwise)
5949
{
60-
leftDrive.drive(steps, amplitude, !clockwise);
61-
rightDrive.drive(steps, calcRightSpeed(amplitude), clockwise);
50+
LeftDrive::drive(steps, amplitude, !clockwise);
51+
RightDrive::drive(steps, calcRightSpeed(amplitude), clockwise);
6252
}
6353

6454
void driveCounter(const Counter steps, const Amplitude amplitude, const bool backwards)
6555
{
66-
leftDrive.drive(steps, amplitude, backwards);
67-
rightDrive.drive(steps, calcRightSpeed(amplitude), backwards);
56+
LeftDrive::drive(steps, amplitude, backwards);
57+
RightDrive::drive(steps, calcRightSpeed(amplitude), backwards);
6858
}
6959

7060
void rotate(const float deg, const Amplitude amplitude, bool const clockwise)
@@ -81,16 +71,9 @@ void drive(const float distance, const Amplitude amplitude, const bool backwards
8171
driveCounter(steps, amplitude, backwards);
8272
}
8373

84-
DriveInterface::DriveInterface() :
85-
counter(0), target(0), isIdle(true) {
86-
}
87-
88-
DriveInterface::~DriveInterface() {
89-
}
90-
9174
IRAM_ATTR void stopDrives() {
92-
leftDrive.stop();
93-
rightDrive.stop();
75+
LeftDrive::stop();
76+
RightDrive::stop();
9477
}
9578

9679
}

‎robot-control-src/Drives.hpp

Lines changed: 17 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -14,89 +14,52 @@ using Amplitude = std::uint16_t;
1414
constexpr Amplitude maxAmplitude = 1023;
1515
constexpr Amplitude cruiseSpeed = maxAmplitude / 2;
1616

17-
void odometryCounter();
18-
19-
class DriveInterface
20-
{
21-
public:
22-
Counter volatile counter;
23-
Counter target;
24-
bool isIdle;
25-
26-
DriveInterface();
27-
virtual void init() = 0;
28-
virtual void evaluateInterval() = 0;
29-
virtual void stop() = 0;
30-
virtual void drive(const Counter distance, const Amplitude amplitude, const bool backwards) = 0;
31-
32-
virtual ~DriveInterface();
33-
};
34-
35-
template<typename MOTORCONTROL, typename DIRECTIONPIN, typename ODOPIN>
36-
class Drive : public DriveInterface
17+
template<typename MOTORCONTROL, MOTORCONTROL& motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN& directionPin, typename ODOPIN, ODOPIN& odoPin>
18+
class Drive
3719
{
3820
public:
21+
static Counter volatile counter;
22+
static Counter target;
23+
static bool isIdle;
3924

40-
Drive(MOTORCONTROL& motorControlPin, DIRECTIONPIN& directionPin, ODOPIN& odoPin) :
41-
motorControlpin(motorControlPin), directionPin(directionPin), odoPin(odoPin) {
42-
43-
}
44-
45-
virtual void init() override
25+
static void init()
4626
{
4727
target = counter = 0;
4828
pinMode(odoPin, INPUT);
49-
attachInterrupt(digitalPinToInterrupt(odoPin), odometryCounter, CHANGE);
29+
attachInterrupt(digitalPinToInterrupt(odoPin), evaluateInterval, FALLING);
5030

5131
analogWrite(motorControlpin, 0);
5232
pinMode(directionPin, OUTPUT);
5333
}
5434

55-
virtual void evaluateInterval() override
35+
IRAM_ATTR static void evaluateInterval()
5636
{
57-
const bool isContact = digitalRead(odoPin) == LOW;
58-
static bool wasContact = isContact;
59-
static Milliseconds intervalStart = millis();
60-
61-
if (wasContact && !isContact) // start interval
62-
{
63-
intervalStart = millis();
64-
} else if (!wasContact && isContact && (millis() - intervalStart) > board::odoMinIntervalDuration) {
65-
// valid interval end
66-
counter++;
67-
if (counter >= target)
68-
stop();
37+
if (++counter >= target)
38+
{
39+
stop();
6940
}
70-
71-
wasContact = isContact;
7241
}
7342

74-
virtual void stop() override
43+
IRAM_ATTR static void stop()
7544
{
7645
analogWrite(motorControlpin, 0);
7746
isIdle = true;
7847
}
7948

80-
virtual void drive(const Counter distance, const Amplitude amplitude, const bool backwards) override
49+
static void drive(const Counter distance, const Amplitude amplitude, const bool backwards)
8150
{
8251
counter = 0;
8352
target = distance;
8453
isIdle = false;
8554
digitalWrite(directionPin, backwards ? LOW : HIGH);
8655
analogWrite(motorControlpin, std::min(maxAmplitude, amplitude));
8756
}
88-
89-
virtual ~Drive() {
90-
}
91-
92-
private:
93-
MOTORCONTROL &motorControlpin;
94-
DIRECTIONPIN &directionPin;
95-
ODOPIN &odoPin;
9657
};
9758

98-
extern DriveInterface& leftDrive;
99-
extern DriveInterface& rightDrive;
59+
#define TYPEANDSYMBOL(t) decltype(t), t
60+
61+
using LeftDrive = Drive<TYPEANDSYMBOL(board::leftMotor), TYPEANDSYMBOL(board::leftBackwards), TYPEANDSYMBOL(board::leftOdoSignal)>;
62+
using RightDrive = Drive<TYPEANDSYMBOL(board::rightMotor), TYPEANDSYMBOL(board::rightBackwards), TYPEANDSYMBOL(board::rightOdoSignal)>;
10063

10164
void rotateCounter(const Counter deg, const Amplitude amplitude, bool const clockwise);
10265
void rotate(const float deg, const Amplitude amplitude, bool const clockwise);

‎robot-control-src/board.cpp

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,15 @@
22
#include <Arduino.h>
33

44
namespace board {
5+
std::uint8_t debugLed = D0;
6+
std::uint8_t leftMotor = D1;
7+
std::uint8_t rightMotor = D2;
8+
std::uint8_t scl = D3;
9+
std::uint8_t sda = D4;
10+
std::uint8_t rightOdoSignal = D5;
11+
std::uint8_t leftOdoSignal = D6;
12+
std::uint8_t ioExpanderIntB = D7;
13+
std::uint8_t ioExpanderIntAInv = D8;
514
constexpr std::uint8_t ioExpander1Address = 0x20;
615
MCP23017 ioExpander1(ioExpander1Address);
716
MCP23017Pin leftBackwards(ioExpander1, 8+0);

‎robot-control-src/board.hpp

Lines changed: 9 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,15 @@ constexpr auto pi = M_PI;
1111

1212
namespace board
1313
{
14-
constexpr std::uint8_t debugLed = D0;
15-
constexpr std::uint8_t leftMotor = D1;
16-
constexpr std::uint8_t rightMotor = D2;
17-
constexpr std::uint8_t scl = D3;
18-
constexpr std::uint8_t sda = D4;
19-
constexpr std::uint8_t rightOdoSignal = D5;
20-
constexpr std::uint8_t leftOdoSignal = D6;
21-
constexpr std::uint8_t ioExpanderIntB = D7;
22-
constexpr std::uint8_t ioExpanderIntAInv = D8;
14+
extern std::uint8_t debugLed;
15+
extern std::uint8_t leftMotor;
16+
extern std::uint8_t rightMotor;
17+
extern std::uint8_t scl;
18+
extern std::uint8_t sda;
19+
extern std::uint8_t rightOdoSignal;
20+
extern std::uint8_t leftOdoSignal;
21+
extern std::uint8_t ioExpanderIntB;
22+
extern std::uint8_t ioExpanderIntAInv;
2323
extern MCP23017 ioExpander1;
2424
extern MCP23017Pin leftBackwards;
2525
extern MCP23017Pin rightBackwards;
@@ -28,8 +28,6 @@ extern MCP23017Pin rightBumper;
2828

2929
constexpr auto maxVelocity = 220; //!< [mm/s]
3030
constexpr float odoIntervalLength = 12*pi; //!< [mm]
31-
//! Minimum time with buffer factor [ms]
32-
constexpr Milliseconds odoMinIntervalDuration = (odoIntervalLength * 1000.0) / (maxVelocity*2.0);
3331
};
3432

3533
#endif // BOARD_HPP

‎robot-control-src/robot-control-src.ino

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -90,8 +90,8 @@ void setup()
9090
pinMode(board::leftBumper, INPUT_PULLUP);
9191
pinMode(board::rightBumper, INPUT_PULLUP);
9292

93-
drives::leftDrive.init();
94-
drives::rightDrive.init();
93+
drives::LeftDrive::init();
94+
drives::RightDrive::init();
9595

9696
Serial.printf("connect to wifi %s ", ssid);
9797
WiFi.begin(ssid, password);
@@ -110,8 +110,8 @@ void setup()
110110
void loop()
111111
{
112112
server.handleClient();
113-
//Serial.printf("left: \t%3u, right: \t%3u\n", drives::leftDrive.counter, drives::rightDrive.counter);
114-
if(drives::leftDrive.isIdle && drives::rightDrive.isIdle && newTarget.isTargetNew)
113+
//Serial.printf("left: \t%3u, right: \t%3u\n", drives::LeftDrive::counter, drives::RightDrive::counter);
114+
if(drives::LeftDrive::isIdle && drives::RightDrive::isIdle && newTarget.isTargetNew)
115115
{
116116
if(newTarget.newDrive!=0)
117117
{

0 commit comments

Comments
 (0)