Skip to content

Commit 54d10b7

Browse files
jonapapjonapap
committed
Merge branch 'master' into digitalSafe
# Conflicts: # src/IO/gpio/Gpio.h
2 parents 406697d + 94ed5ce commit 54d10b7

File tree

14 files changed

+237
-18
lines changed

14 files changed

+237
-18
lines changed

‎src/IO/ArduinoProxy.cpp‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include "IO/ArduinoEncoder.h"
66
#include <spdlog/spdlog.h>
77
#include <sys/poll.h>
8+
#include <thread>
89

910
// If we haven't received a pin update in that duration, remove it from the map
1011
const auto pinStateTimeout = std::chrono::seconds(5);
@@ -23,12 +24,20 @@ void ArduinoProxy::initialize()
2324
{
2425
std::lock_guard<std::mutex> lockGuard(serialMutex);
2526

27+
#if DESKTOP_COMPAT == 1
28+
if ((fd = serialOpen("/dev/ttyACM0", 57600)) < 0)
29+
#else
2630
if ((fd = serialOpen("/dev/ttyAMA0", 57600)) < 0)
31+
#endif
2732
{
2833
SPDLOG_LOGGER_ERROR(logger, "Error while opening serial communication!");
2934
return;
3035
}
3136

37+
#if DESKTOP_COMPAT == 1
38+
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
39+
#endif
40+
3241
inititialized = true;
3342

3443
IO::initialize();
@@ -108,6 +117,15 @@ void ArduinoProxy::handleArduinoMessage(const RocketryProto::ArduinoOut &arduino
108117
digitalStates[digitalState.pin()] = {digitalState.activated(), std::chrono::steady_clock::now()};
109118
}
110119
break;
120+
case RocketryProto::ArduinoOut::kDcMotorState: {
121+
std::lock_guard<std::mutex> lockGuard(stateMutex);
122+
123+
const auto &dcMotorState = arduinoOut.dcmotorstate();
124+
dcMotorStates[{dcMotorState.motorforwardpin(), dcMotorState.motorreversepin()}] = {
125+
dcMotorState.position(), dcMotorState.direction(), dcMotorState.minlimitswitch(),
126+
dcMotorState.maxlimitswitch(), std::chrono::steady_clock::now()};
127+
}
128+
break;
111129
case RocketryProto::ArduinoOut::DATA_NOT_SET:
112130
SPDLOG_LOGGER_WARN(logger, "Data field not set in Arduino message. ");
113131
break;
@@ -174,4 +192,23 @@ int ArduinoProxy::getServoState(int pin)
174192
}
175193
}
176194

195+
DCMotorState ArduinoProxy::getDCMotorState(int forwardPin, int reversePin)
196+
{
197+
std::lock_guard<std::mutex> lockGuard(stateMutex);
198+
199+
auto state = dcMotorStates.at({forwardPin, reversePin});
200+
auto now = std::chrono::steady_clock::now();
201+
202+
if (now - state.time >= pinStateTimeout)
203+
{
204+
dcMotorStates.erase({forwardPin, reversePin});
205+
SPDLOG_ERROR("Arduino stopped reporting DC motor pin {} and {}", forwardPin, reversePin);
206+
throw std::out_of_range("Pin has been removed");
207+
}
208+
else
209+
{
210+
return state;
211+
}
212+
}
213+
177214
#endif

‎src/IO/ArduinoProxy.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#if USE_ARDUINO_PROXY == 1
55

66
#include "IO.h"
7+
#include "arduino/DCMotorState.h"
78
#include <ArduinoComm.pb.h>
89
#include <wiringSerial.h>
910

@@ -23,13 +24,15 @@ class ArduinoProxy : IO
2324

2425
bool getDigitalState(int pin);
2526
int getServoState(int pin);
27+
DCMotorState getDCMotorState(int forwardPin, int reversePin);
2628

2729
ArduinoProxy(ArduinoProxy const &) = delete;
2830
void operator=(ArduinoProxy const &) = delete;
2931

3032
private:
3133
std::map<unsigned int, std::pair<bool, std::chrono::time_point<std::chrono::steady_clock>>> digitalStates;
3234
std::map<unsigned int, std::pair<int, std::chrono::time_point<std::chrono::steady_clock>>> servoStates;
35+
std::map<std::pair<unsigned int, unsigned int>, DCMotorState> dcMotorStates;
3336
std::mutex stateMutex;
3437

3538
int fd = 0;

‎src/IO/Interface.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@ class Interface
2727

2828
virtual void createNewGpioPwmOutput(std::string name, int pinNbr, int safePosition, bool softpwm) = 0;
2929

30+
virtual void createNewGpioDCMotorOutput(std::string name, int pinForward, int pinReverse, int motorPower,
31+
int limitSwitchMinPin, int limitSwitchMaxPin, int potentiometerPin) = 0;
32+
3033
#endif
3134

3235
#if USE_LOGGER == 1

‎src/IO/InterfaceImpl.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -170,6 +170,13 @@ void InterfaceImpl::createNewGpioPwmOutput(std::string name, int pinNbr, int saf
170170
{
171171
gpio.createNewGpioPwmOutput(name, pinNbr, safePosition, softpwm);
172172
}
173+
174+
void InterfaceImpl::createNewGpioDCMotorOutput(std::string name, int pinForward, int pinReverse, int motorPower,
175+
int limitSwitchMinPin, int limitSwitchMaxPin, int potentiometerPin)
176+
{
177+
gpio.createNewGpioDCMotorOutput(name, pinForward, pinReverse, motorPower, limitSwitchMinPin, limitSwitchMaxPin,
178+
potentiometerPin);
179+
}
173180
#endif
174181

175182
void InterfaceImpl::calibrateTelemetry()

‎src/IO/InterfaceImpl.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,8 @@ class InterfaceImpl : public Interface
3838
#if USE_GPIO == 1
3939
void createNewGpioOutput(std::string name, int pinNbr, int safeState = 0) override;
4040
void createNewGpioPwmOutput(std::string name, int pinNbr, int safePosition, bool softpwm) override;
41+
void createNewGpioDCMotorOutput(std::string name, int pinForward, int pinReverse, int motorPower,
42+
int limitSwitchMinPin, int limitSwitchMaxPin, int potentiometerPin) override;
4143
#endif
4244

4345
#if USE_LOGGER == 1

‎src/IO/gpio/DCMotor.cpp‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
#include "config.h"
2+
3+
#if USE_GPIO == 1
4+
5+
#include "DCMotor.h"
6+
#include <iostream>
7+
#include <spdlog/spdlog.h>
8+
#include <utility>
9+
#include <wiringSerial.h>
10+
11+
DCMotorOutput::DCMotorOutput(std::string name, int pinForward, int pinReverse, int motorPower, int limitSwitchMinPin,
12+
int limitSwitchMaxPin, int potentiometerPin)
13+
: name(std::move(name)), pinNbrForward(pinForward), pinNbrReverse(pinReverse), motorPower(motorPower),
14+
limitSwitchMinPin(limitSwitchMinPin), limitSwitchMaxPin(limitSwitchMaxPin), potentiometerPin(potentiometerPin)
15+
{
16+
logger = spdlog::default_logger();
17+
18+
SPDLOG_LOGGER_DEBUG(logger, "Created PwmOutput {}", name);
19+
20+
#if USE_ARDUINO_PROXY == 1
21+
arduinoProxy = ArduinoProxy::getInstance();
22+
23+
RocketryProto::ArduinoIn arduinoIn;
24+
arduinoIn.mutable_dcmotorinit()->set_motorforwardpin(pinForward);
25+
arduinoIn.mutable_dcmotorinit()->set_motorreversepin(pinReverse);
26+
arduinoIn.mutable_dcmotorinit()->set_motorpower(motorPower);
27+
arduinoIn.mutable_dcmotorinit()->set_limitswitchminpin(limitSwitchMinPin);
28+
arduinoIn.mutable_dcmotorinit()->set_limitswitchmaxpin(limitSwitchMaxPin);
29+
arduinoIn.mutable_dcmotorinit()->set_potentiometerpin(potentiometerPin);
30+
arduinoProxy->send(arduinoIn);
31+
#endif
32+
}
33+
34+
bool DCMotorOutput::setValue(int value)
35+
{
36+
if (currentState != value)
37+
{
38+
currentState = value;
39+
SPDLOG_LOGGER_INFO(logger, "DC Motor {} changed to {} on pin {} and {}", name, currentState, pinNbrForward,
40+
pinNbrReverse);
41+
42+
#if USE_ARDUINO_PROXY == 1
43+
RocketryProto::ArduinoIn arduinoIn;
44+
arduinoIn.mutable_dcmotorcontrol()->set_pinforward(pinNbrForward);
45+
arduinoIn.mutable_dcmotorcontrol()->set_pinreverse(pinNbrReverse);
46+
arduinoIn.mutable_dcmotorcontrol()->set_position(value);
47+
48+
arduinoProxy->send(arduinoIn);
49+
#endif
50+
}
51+
return true;
52+
}
53+
54+
DCMotorState DCMotorOutput::getCurrentState()
55+
{
56+
#if USE_ARDUINO_PROXY == 1
57+
try
58+
{
59+
return arduinoProxy->getDCMotorState(pinNbrForward, pinNbrReverse);
60+
}
61+
catch (std::out_of_range &error)
62+
{
63+
return {-1, 0, false, false, std::chrono::steady_clock::now()};
64+
}
65+
#else
66+
return {-1, 0, false, false, std::chrono::steady_clock::now()};
67+
#endif
68+
}
69+
70+
#endif

‎src/IO/gpio/DCMotor.h‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#pragma once
2+
3+
#include "config.h"
4+
5+
#if USE_GPIO == 1
6+
7+
#include "Output.h"
8+
#include "arduino/DCMotorState.h"
9+
10+
#if USE_ARDUINO_PROXY
11+
#include "IO/ArduinoProxy.h"
12+
#endif
13+
14+
#include <spdlog/logger.h>
15+
#include <string>
16+
17+
class DCMotorOutput : public Output
18+
{
19+
public:
20+
DCMotorOutput(std::string name, int pinForward, int pinReverse, int motorPower, int limitSwitchMinPin,
21+
int limitSwitchMaxPin, int potentiometerPin);
22+
23+
bool setValue(int value) override;
24+
25+
DCMotorState getCurrentState();
26+
27+
private:
28+
std::shared_ptr<spdlog::logger> logger;
29+
const std::string name;
30+
const int pinNbrForward;
31+
const int pinNbrReverse;
32+
const int motorPower;
33+
const int limitSwitchMinPin;
34+
const int limitSwitchMaxPin;
35+
const int potentiometerPin;
36+
37+
#if USE_ARDUINO_PROXY == 1
38+
ArduinoProxy *arduinoProxy;
39+
#endif
40+
};
41+
42+
#endif

‎src/IO/gpio/Gpio.cpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,10 +45,15 @@ GpioData Gpio::setOutputs(const GpioData &data)
4545
{
4646
digitalOutputsMap.at(output.first).setValue(output.second);
4747
}
48+
for (std::pair<std::string, int> output : data.dcOutputMap)
49+
{
50+
dcMotorOutputsMap.at(output.first).setValue(output.second);
51+
}
4852

4953
GpioData result;
5054
result.digitalOutputMap = toRawMap(digitalOutputsMap);
5155
result.pwmOutputMap = toRawMap(pwmOutputsMap);
56+
result.dcOutputMap = toRawMap(dcMotorOutputsMap);
5257

5358
return result;
5459
}
@@ -63,6 +68,13 @@ void Gpio::createNewGpioPwmOutput(const std::string &name, int pinNbr, int safeP
6368
pwmOutputsMap.insert({name, PwmOutput(name, pinNbr, safePosition, softPWM)});
6469
}
6570

71+
void Gpio::createNewGpioDCMotorOutput(const std::string &name, int pinForward, int pinReverse, int motorPower,
72+
int limitSwitchMinPin, int limitSwitchMaxPin, int potentiometerPin)
73+
{
74+
dcMotorOutputsMap.insert({name, DCMotorOutput(name, pinForward, pinReverse, motorPower, limitSwitchMinPin,
75+
limitSwitchMaxPin, potentiometerPin)});
76+
}
77+
6678
GpioState Gpio::getCurrentState()
6779
{
6880
GpioState state;
@@ -75,6 +87,10 @@ GpioState Gpio::getCurrentState()
7587
{
7688
state.pwmStateMap.insert({i.first, i.second.getCurrentState()});
7789
}
90+
for (auto i : dcMotorOutputsMap)
91+
{
92+
state.dcMotorStateMap.insert({i.first, i.second.getCurrentState()});
93+
}
7894

7995
return state;
8096
}

‎src/IO/gpio/Gpio.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#if USE_GPIO == 1
55

66
#include "IO/IO.h"
7+
#include "IO/gpio/DCMotor.h"
78
#include "IO/gpio/DigitalOutput.h"
89
#include "IO/gpio/PwmOutput.h"
910
#include "data/GpioData.h"
@@ -28,8 +29,9 @@ class Gpio : public IO
2829
bool isInitialized() override;
2930

3031
void createNewGpioOutput(const std::string &name, int pinNbr, int safeState);
31-
3232
void createNewGpioPwmOutput(const std::string &name, int pinNbr, int safePosition, bool softPWM = false);
33+
void createNewGpioDCMotorOutput(const std::string &name, int pinForward, int pinReverse, int motorPower,
34+
int limitSwitchMinPin, int limitSwitchMaxPin, int potentiometerPin);
3335

3436
GpioData setOutputs(const GpioData &data);
3537

@@ -41,6 +43,7 @@ class Gpio : public IO
4143
private:
4244
std::map<std::string, DigitalOutput> digitalOutputsMap;
4345
std::map<std::string, PwmOutput> pwmOutputsMap;
46+
std::map<std::string, DCMotorOutput> dcMotorOutputsMap;
4447

4548
struct InitFlags
4649
{

‎src/arduino/DCMotorState.h‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
#pragma once
2+
3+
#include <chrono>
4+
5+
struct DCMotorState
6+
{
7+
int position;
8+
int direction;
9+
bool minLimitSwitch;
10+
bool maxLimitSwitch;
11+
std::chrono::time_point<std::chrono::steady_clock> time;
12+
};

0 commit comments

Comments
 (0)