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

Commit 0d367ef

Browse files
dhebbekerdhebbeker
committed
Adjust Drive such that it takes pins as runtime objects instead of template arguments.
1 parent bcba139 commit 0d367ef

File tree

5 files changed

+135
-77
lines changed

5 files changed

+135
-77
lines changed

‎robot-control-src/Drives.cpp

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,28 @@
11
#include "Drives.hpp"
2+
#include "board.hpp"
3+
4+
#include <cmath>
5+
6+
#undef round //see https://github.com/esp8266/Arduino/issues/5787#issuecomment-465852231
7+
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+
}
213

314
namespace drives
415
{
16+
static auto l = makeDrive(board::leftMotor, board::leftBackwards, board::leftOdoSignal);
17+
static auto r = makeDrive(board::rightMotor, board::rightBackwards, board::rightOdoSignal);
518

6-
template<int A, int B, int C> std::uint16_t volatile Drive<A, B, C>::counter = 0;
7-
template<int A, int B, int C> std::uint16_t Drive<A, B, C>::target = 0;
8-
template<int A, int B, int C> bool Drive<A, B, C>::isIdle = true;
19+
DriveInterface& leftDrive = l;
20+
DriveInterface& rightDrive = r;
921

10-
ICACHE_RAM_ATTR void odometryCounter()
22+
IRAM_ATTR void odometryCounter()
1123
{
12-
LeftDrive::evaluateInterval();
13-
RightDrive::evaluateInterval();
24+
leftDrive.evaluateInterval();
25+
rightDrive.evaluateInterval();
1426
}
1527

1628
/**
@@ -45,14 +57,14 @@ static Amplitude calcRightSpeed(const Amplitude leftSpeed)
4557

4658
void rotateCounter(const Counter steps, const Amplitude amplitude, bool const clockwise)
4759
{
48-
LeftDrive::drive(steps, amplitude, !clockwise);
49-
RightDrive::drive(steps, calcRightSpeed(amplitude), clockwise);
60+
leftDrive.drive(steps, amplitude, !clockwise);
61+
rightDrive.drive(steps, calcRightSpeed(amplitude), clockwise);
5062
}
5163

5264
void driveCounter(const Counter steps, const Amplitude amplitude, const bool backwards)
5365
{
54-
LeftDrive::drive(steps, amplitude, backwards);
55-
RightDrive::drive(steps, calcRightSpeed(amplitude), backwards);
66+
leftDrive.drive(steps, amplitude, backwards);
67+
rightDrive.drive(steps, calcRightSpeed(amplitude), backwards);
5668
}
5769

5870
void rotate(const float deg, const Amplitude amplitude, bool const clockwise)
@@ -68,4 +80,14 @@ void drive(const float distance, const Amplitude amplitude, const bool backwards
6880
const Counter steps = distance * stepsPerMm;
6981
driveCounter(steps, amplitude, backwards);
7082
}
83+
84+
DriveInterface::DriveInterface() :
85+
counter(0), target(0), isIdle(true) {
86+
}
87+
88+
DriveInterface::~DriveInterface() {
89+
}
90+
7191
}
92+
93+

‎robot-control-src/Drives.hpp

Lines changed: 79 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,9 @@
22
#define DRIVES_HPP
33

44
#include "board.hpp"
5+
#include <algorithm>
56
#include <cstdint>
6-
#include <cmath>
7+
#include <Arduino.h>
78

89
namespace drives
910
{
@@ -15,64 +16,87 @@ constexpr Amplitude cruiseSpeed = maxAmplitude / 2;
1516

1617
void odometryCounter();
1718

18-
template<int MotorControl, int DirectionPin, int OdoPin>
19-
struct Drive
19+
class DriveInterface
2020
{
21-
22-
static Counter volatile counter;
23-
static Counter target;
24-
static bool isIdle;
25-
26-
static void init()
27-
{
28-
target = counter = 0;
29-
pinMode(OdoPin, INPUT);
30-
attachInterrupt(digitalPinToInterrupt(OdoPin), odometryCounter, CHANGE);
31-
32-
//pinMode(MotorControl, OUTPUT);
33-
analogWrite(MotorControl, 0);
34-
pinMode(DirectionPin, OUTPUT);
35-
}
36-
37-
static void evaluateInterval()
38-
{
39-
const bool isContact = digitalRead(OdoPin) == LOW;
40-
static bool wasContact = isContact;
41-
static Milliseconds intervalStart = millis();
42-
43-
if(wasContact && !isContact) // start interval
44-
{
45-
intervalStart = millis();
46-
}
47-
else if(!wasContact && isContact && (millis() - intervalStart) > board::odoMinIntervalDuration)
48-
{
49-
// valid interval end
50-
counter++;
51-
if(counter>=target) stop();
52-
}
53-
54-
wasContact = isContact;
55-
}
56-
57-
static void stop()
58-
{
59-
analogWrite(MotorControl, 0);
60-
isIdle = true;
61-
}
62-
63-
static void drive(const Counter distance, const Amplitude amplitude, const bool backwards)
64-
{
65-
counter = 0;
66-
target = distance;
67-
isIdle = false;
68-
digitalWrite(DirectionPin, backwards ? LOW : HIGH);
69-
analogWrite(MotorControl, std::min(maxAmplitude, amplitude));
70-
}
21+
public:
22+
Counter volatile counter;
23+
Counter target;
24+
bool isIdle;
7125

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
37+
{
38+
public:
39+
40+
Drive(MOTORCONTROL& motorControlPin, DIRECTIONPIN& directionPin, ODOPIN& odoPin) :
41+
motorControlpin(motorControlPin), directionPin(directionPin), odoPin(odoPin) {
42+
43+
}
44+
45+
virtual void init() override
46+
{
47+
target = counter = 0;
48+
pinMode(odoPin, INPUT);
49+
attachInterrupt(digitalPinToInterrupt(odoPin), odometryCounter, CHANGE);
50+
51+
analogWrite(motorControlpin, 0);
52+
pinMode(directionPin, OUTPUT);
53+
}
54+
55+
virtual void evaluateInterval() override
56+
{
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();
69+
}
70+
71+
wasContact = isContact;
72+
}
73+
74+
virtual void stop() override
75+
{
76+
analogWrite(motorControlpin, 0);
77+
isIdle = true;
78+
}
79+
80+
virtual void drive(const Counter distance, const Amplitude amplitude, const bool backwards) override
81+
{
82+
counter = 0;
83+
target = distance;
84+
isIdle = false;
85+
digitalWrite(directionPin, backwards ? LOW : HIGH);
86+
analogWrite(motorControlpin, std::min(maxAmplitude, amplitude));
87+
}
88+
89+
virtual ~Drive() {
90+
}
91+
92+
private:
93+
MOTORCONTROL &motorControlpin;
94+
DIRECTIONPIN &directionPin;
95+
ODOPIN &odoPin;
7296
};
7397

74-
using LeftDrive = Drive<board::leftMotor, board::leftBackwards, board::leftOdoSignal>;
75-
using RightDrive = Drive<board::rightMotor, board::rightBackwards, board::rightOdoSignal>;
98+
extern DriveInterface& leftDrive;
99+
extern DriveInterface& rightDrive;
76100

77101
void rotateCounter(const Counter deg, const Amplitude amplitude, bool const clockwise);
78102
void rotate(const float deg, const Amplitude amplitude, bool const clockwise);

‎robot-control-src/board.cpp

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
#include "board.hpp"
2+
#include <Arduino.h>
3+
4+
namespace board {
5+
decltype(leftOdoSignal) leftOdoSignal;
6+
decltype(rightOdoSignal) rightOdoSignal;
7+
decltype(rightBackwards) rightBackwards;
8+
decltype(leftBackwards) leftBackwards;
9+
decltype(leftMotor) leftMotor = D1;
10+
decltype(rightMotor) rightMotor = D2;
11+
decltype(debugLed) debugLed = D0;
12+
};

‎robot-control-src/board.hpp

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,21 @@
11
#if !defined(BOARD_HPP)
22
#define BOARD_HPP
3+
#include "DigitalPin.hpp"
34
#include <Arduino.h>
4-
#undef round //see https://github.com/esp8266/Arduino/issues/5787#issuecomment-465852231
55
#include <cmath>
66

77
typedef decltype(millis()) Milliseconds;
88
constexpr auto pi = std::acos(-1);
99

1010
namespace board
1111
{
12-
constexpr auto leftOdoSignal = D6; //!< blue
13-
constexpr auto rightOdoSignal = D7; //!< green
14-
constexpr auto rightBackwards = D5; //!< yellow
15-
constexpr auto leftBackwards = D3; //!< white
16-
constexpr auto leftMotor = D1; //!< brown
17-
constexpr auto rightMotor = D2; //!< orange
18-
constexpr auto debugLed = D4;
12+
extern DigitalPin<D6> leftOdoSignal;
13+
extern DigitalPin<D5> rightOdoSignal;
14+
extern DigitalPin<D5> rightBackwards;
15+
extern DigitalPin<D5> leftBackwards;
16+
extern const std::uint8_t leftMotor;
17+
extern const std::uint8_t rightMotor;
18+
extern const std::uint8_t debugLed;
1919

2020
constexpr auto maxVelocity = 220; //!< [mm/s]
2121
constexpr float odoIntervalLength = 12*pi; //!< [mm]

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -84,8 +84,8 @@ void setup()
8484
Serial.printf("\n begin program '%s'\n", __FILE__);
8585
pinMode(board::debugLed, OUTPUT);
8686
digitalWrite(board::debugLed, LOW);
87-
drives::LeftDrive::init();
88-
drives::RightDrive::init();
87+
drives::leftDrive.init();
88+
drives::rightDrive.init();
8989

9090
Serial.printf("connect to wifi %s ", ssid);
9191
WiFi.begin(ssid, password);
@@ -104,8 +104,8 @@ void setup()
104104
void loop()
105105
{
106106
server.handleClient();
107-
//Serial.printf("left: \t%3u, right: \t%3u\n", drives::LeftDrive::counter, drives::RightDrive::counter);
108-
if(drives::LeftDrive::isIdle && drives::RightDrive::isIdle && newTarget.isTargetNew)
107+
//Serial.printf("left: \t%3u, right: \t%3u\n", drives::leftDrive.counter, drives::rightDrive.counter);
108+
if(drives::leftDrive.isIdle && drives::rightDrive.isIdle && newTarget.isTargetNew)
109109
{
110110
if(newTarget.newDrive!=0)
111111
{

0 commit comments

Comments
 (0)