Skip to content

Commit d65d8f2

Browse files
genemarsgenemars
committed
- added *Motion Control* and *Steering Control* modules to FPV-Car firmware
- added default motors settings to FPV-Car / Robot Arm firmwares - improved dynamic memory allocation checks (PSRAM/DRAM)
1 parent b3c10bd commit d65d8f2

20 files changed

Lines changed: 336 additions & 138 deletions

‎examples/smart-motor/README.md‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -13,13 +13,25 @@ The default version of this firmware can control up to 8 motors simultaneously.
1313
In addition to default system options the following configuration options are available,
1414
where `<n>` is the number of the motor (from 1 to 8):
1515

16-
| Key | Description | Default |
17-
|----------------|----------------------------------------|-----------------------------------------|
18-
| `stld-pin` | Status LED (RGB) pin | -1 (-1=not used) |
19-
| `stld-typ` | Status LED type | RGB/RGBW order mask (see code for ref.) |
20-
| `stld-spd` | Status LED speed | 0 (0=800kHz, 256=400kHz) |
21-
| `motr-typ#<n>` | Motor type ('Servo' or 'ServoEncoder') | 'Servo' |
22-
| `motr-pin#<n>` | Motor signal GPIO# | 6 |
16+
| Key | Description | Default |
17+
|----------------|------------------------------------------------|-----------------------------------------|
18+
| `stld-pin` | Status LED (RGB) pin | -1 (-1=not used) |
19+
| `stld-typ` | Status LED type | RGB/RGBW order mask (see code for ref.) |
20+
| `stld-spd` | Status LED speed | 0 (0=800kHz, 256=400kHz) |
21+
| `motr-typ#<n>` | Motor type (Servo or ServoEncoder) | Servo |
22+
| `motr-pin#<n>` | Motor signal GPIO# | 6 |
23+
| `rcrcar-4wd` | Enable configuration for FPV-RC-Car project | |
24+
| `robotc-arm` | Enable configuration for Robot-Arm project | |
25+
26+
27+
When `rcrcar-4wd` is set to `true` motors `S1`, `S2`, `S3`, `S4` will be configured to
28+
operate at maximum speed (10). It will also add `Motion Control` and `Steering Control`
29+
modules for precise control of the *FPV-RC-Car* motion and steering.
30+
31+
When `robotc-arm` is set to `true` and also `rcrcar-4wd` is set to `true`, then
32+
motors `S5`, `S6`, `S7`, `S8` will be configured to operate at half speed (5) to
33+
enable smoother *Robotic Arm* movements. Otherwise, if `rcrcar-4wd` is set to `false`
34+
then the configured motors will be `S1`, `S2`, `S3`, `S4`.
2335

2436

2537
### Parameters for `motr-typ`= '`ServoEncoder`'

‎examples/smart-motor/api/MotorHandler.cpp‎

Lines changed: 73 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -66,75 +66,80 @@ namespace Service { namespace API {
6666
// "|motor_stepper=Stepper"
6767
)->withConfigKey(K(MotorType, i))->addUpdateListener(optionUpdateListener);
6868

69-
// Servo options
69+
auto type = Config::getSetting(K(MotorType, i));
70+
if (type.equals("ServoEncoder")) { // "ServoEncoder"
71+
72+
// ServoEncoder options
73+
motorModule->addWidgetOption(
74+
// name, value
75+
Options::Motor_ServoEncoder_Speed, "5",
76+
// field type
77+
UI_WIDGETS_FIELD_TYPE_NUMBER
78+
// label
79+
":speed"
80+
// min:max:default
81+
":1:10:5"
82+
)->withConfigKey(K(MotorSpeed, i))->addUpdateListener(optionUpdateListener);
83+
84+
motorModule->addWidgetOption(
85+
// name, value
86+
Options::Motor_ServoEncoder_Steps, "15",
87+
// field type
88+
UI_WIDGETS_FIELD_TYPE_NUMBER
89+
// label
90+
":motor_steps"
91+
// min:max:default
92+
":1:100:15"
93+
)->withConfigKey(K(MotorSteps, i))->addUpdateListener(optionUpdateListener);
94+
95+
motorModule->addWidgetOption(
96+
// name, value (shown as button text)
97+
Options::Motor_ServoEncoder_Calibrate, Calibration::StartLabel,
98+
// field type
99+
UI_WIDGETS_FIELD_TYPE_BUTTON
100+
// label
101+
":calibration"
102+
// API command to call on click
103+
":Motor.Calibrate"
104+
)->addUpdateListener(optionUpdateListener);
105+
106+
} else { // "Servo" or default
107+
108+
// Servo options
109+
motorModule->addWidgetOption(
110+
// name, value
111+
Options::Motor_Servo_AngleSpeed, "10",
112+
// field type
113+
UI_WIDGETS_FIELD_TYPE_NUMBER
114+
// label
115+
":speed"
116+
// min:max:default
117+
":1:10:10"
118+
)->withConfigKey(K(AngleSpeed, i))->addUpdateListener(optionUpdateListener);
119+
120+
motorModule->addWidgetOption(
121+
// name, value
122+
Options::Motor_Servo_AngleMin, "0",
123+
// field type
124+
UI_WIDGETS_FIELD_TYPE_NUMBER
125+
// label
126+
":angle_min_value"
127+
// min:max:default
128+
":0:180:0"
129+
)->withConfigKey(K(AngleMin, i))->addUpdateListener(optionUpdateListener);
130+
131+
motorModule->addWidgetOption(
132+
// name, value
133+
Options::Motor_Servo_AngleMax, "180",
134+
// field type
135+
UI_WIDGETS_FIELD_TYPE_NUMBER
136+
// label
137+
":angle_max_value"
138+
// min:max:default
139+
":0:180:180"
140+
)->withConfigKey(K(AngleMax, i))->addUpdateListener(optionUpdateListener);
70141

71-
motorModule->addWidgetOption(
72-
// name, value
73-
Options::Motor_Servo_AngleSpeed, "5",
74-
// field type
75-
UI_WIDGETS_FIELD_TYPE_NUMBER
76-
// label
77-
":speed"
78-
// min:max:default
79-
":1:10:5"
80-
)->withConfigKey(K(AngleSpeed, i))->addUpdateListener(optionUpdateListener);
81-
82-
motorModule->addWidgetOption(
83-
// name, value
84-
Options::Motor_Servo_AngleMin, "0",
85-
// field type
86-
UI_WIDGETS_FIELD_TYPE_NUMBER
87-
// label
88-
":angle_min_value"
89-
// min:max:default
90-
":0:180:0"
91-
)->withConfigKey(K(AngleMin, i))->addUpdateListener(optionUpdateListener);
92-
93-
motorModule->addWidgetOption(
94-
// name, value
95-
Options::Motor_Servo_AngleMax, "180",
96-
// field type
97-
UI_WIDGETS_FIELD_TYPE_NUMBER
98-
// label
99-
":angle_max_value"
100-
// min:max:default
101-
":0:180:180"
102-
)->withConfigKey(K(AngleMax, i))->addUpdateListener(optionUpdateListener);
103-
104-
105-
// ServoEncoder options
106-
motorModule->addWidgetOption(
107-
// name, value
108-
Options::Motor_ServoEncoder_Speed, "5",
109-
// field type
110-
UI_WIDGETS_FIELD_TYPE_NUMBER
111-
// label
112-
":speed"
113-
// min:max:default
114-
":1:10:5"
115-
)->withConfigKey(K(MotorSpeed, i))->addUpdateListener(optionUpdateListener);
116-
117-
motorModule->addWidgetOption(
118-
// name, value
119-
Options::Motor_ServoEncoder_Steps, "15",
120-
// field type
121-
UI_WIDGETS_FIELD_TYPE_NUMBER
122-
// label
123-
":motor_steps"
124-
// min:max:default
125-
":1:100:15"
126-
)->withConfigKey(K(MotorSteps, i))->addUpdateListener(optionUpdateListener);
127-
128-
motorModule->addWidgetOption(
129-
// name, value (shown as button text)
130-
Options::Motor_ServoEncoder_Calibrate, Calibration::StartLabel,
131-
// field type
132-
UI_WIDGETS_FIELD_TYPE_BUTTON
133-
// label
134-
":calibration"
135-
// API command to call on click
136-
":Motor.Calibrate"
137-
)->addUpdateListener(optionUpdateListener);
142+
}
138143

139144
}
140145

‎examples/smart-motor/io/drivers/ServoDriver.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ namespace IO { namespace Components {
5959
int stopUs = 1500;
6060

6161
const int stepSpeedMax = 10;
62-
int stepSpeed = 5; // [1 .. <stepSpeedMax>]
62+
int stepSpeed = 10; // [1 .. <stepSpeedMax>]
6363

6464
float minAngle = 0;
6565
float maxAngle = 180;
@@ -94,7 +94,7 @@ namespace IO { namespace Components {
9494

9595
// Read stored config or apply default values.
9696
auto k = K(AngleSpeed, idx);
97-
configure(k, Config::getSetting(k, "5").c_str());
97+
configure(k, Config::getSetting(k, "10").c_str());
9898
k = K(AngleMin, idx);
9999
configure(k, Config::getSetting(k, "0").c_str());
100100
k = K(AngleMax, idx);

‎examples/smart-motor/smart-motor.cpp‎

Lines changed: 139 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
#include "configuration.h"
3131

3232
#include <HomeGenie.h>
33+
#include <service/api/devices/Dimmer.h>
3334

3435
#include "api/MotorHandler.h"
3536

@@ -40,9 +41,15 @@ StatusLed statusLed;
4041

4142
using namespace IO;
4243
using namespace Service;
44+
using namespace Service::API::devices;
4345

4446
HomeGenie* homeGenie;
4547

48+
MotorHandler* motorHandler;
49+
bool is4wdRcCar;
50+
bool isRoboticArm;
51+
bool roboArmInitialized = false;
52+
4653
void setup() {
4754
// Default name shown in SNMP/UPnP advertising
4855
Config::system.friendlyName = "Smart Motor";
@@ -54,13 +61,144 @@ void setup() {
5461
statusLed.setup();
5562
#endif
5663

57-
auto motorHandler = new MotorHandler();
64+
motorHandler = new MotorHandler();
5865
homeGenie->addAPIHandler(motorHandler);
5966

6067
homeGenie->begin();
68+
69+
// Adds custom control modules
70+
71+
is4wdRcCar = Config::getSetting("rc-car-4wd").equals("true");
72+
if (is4wdRcCar) {
73+
74+
// Add *Motion* and *Steering* control modules if this device is for the
75+
// FPV 4WD RC Car https://homegenie.it/mini/1.2/examples/fpv-rc-car/
76+
77+
// RC Car top view:
78+
//
79+
// (S4)-----(S3) - front wheels
80+
// | ... |
81+
// | ... |
82+
// (S2)-----(S1) - rear wheels (inverted mounting direction)
83+
//
84+
// - S1 = Servo Motor 1
85+
// - S2 = Servo Motor 2
86+
// - S3 = Servo Motor 3
87+
// - S4 = Servo Motor 4
88+
89+
auto list = motorHandler->getModuleList();
90+
if (list->size() >= 4) {
91+
for (auto i = 0; i < 4; i++) {
92+
list->get(i)->setProperty(WidgetApi::Preference::HomeLevel, "50");
93+
list->get(i)->setProperty(WidgetApi::Preference::AutoHome, "true");
94+
}
95+
// Module S1 to S4 are the 4WD motors
96+
auto motionControl = new Dimmer(IO::IOEventDomains::Automation_Components, "MT1", "Motion Control");
97+
motionControl->module->type = ModuleType::Motor;
98+
motionControl->module->setProperty(WidgetApi::DisplayModule, "homegenie/generic/shutter");
99+
motionControl->module->setProperty(WidgetApi::Preference::HomeLevel, "50");
100+
motionControl->module->setProperty(WidgetApi::Preference::AutoHome, "true");
101+
motionControl->module->setProperty(WidgetApi::Preference::LevelStep, "2");
102+
motionControl->setDefaultTransition(0);
103+
motionControl->setLevel(0.5f);
104+
motionControl->onSetLevel([list](float level) {
105+
static float currentLevel = 0;
106+
if (level == currentLevel) {
107+
return;
108+
}
109+
currentLevel = level;
110+
// move all 4WD motors
111+
// > 0.5 to 1.0 forward
112+
// < 0.5 to 0.0 backward
113+
// = 0.5 stopped
114+
auto s1 = list->get(0);
115+
auto s2 = list->get(1);
116+
auto s3 = list->get(2);
117+
auto s4 = list->get(3);
118+
level *= 100;
119+
((MotorModule*) s1)->motorControl->setLevel(100 - level);
120+
((MotorModule*) s2)->motorControl->setLevel(level);
121+
((MotorModule*) s3)->motorControl->setLevel(100 - level);
122+
((MotorModule*) s4)->motorControl->setLevel(level);
123+
});
124+
HomeGenie::getInstance()->addAPIHandler(motionControl);
125+
auto steerControl = new Dimmer(IO::IOEventDomains::Automation_Components, "ST1", "Steering Control");
126+
steerControl->module->type = ModuleType::Motor;
127+
steerControl->module->setProperty(WidgetApi::DisplayModule, "homegenie/generic/shutter");
128+
steerControl->module->setProperty(WidgetApi::Preference::HomeLevel, "50");
129+
steerControl->module->setProperty(WidgetApi::Preference::AutoHome, "true");
130+
steerControl->module->setProperty(WidgetApi::Preference::LevelStep, "2");
131+
steerControl->setDefaultTransition(0);
132+
steerControl->setLevel(0.5f);
133+
steerControl->onSetLevel([list](float level) {
134+
static float currentLevel = 0;
135+
Serial.println(level);
136+
if (level == currentLevel) {
137+
return;
138+
}
139+
currentLevel = level;
140+
// Steering system
141+
// > 0.5 to 1.0 - steer right : move S1,S3 backward and S2,S4 forward
142+
// < 0.5 to 0.0 - steer left : move S1,S3 forward and S2,S4 backward
143+
// = 0.5 stopped
144+
auto s1 = list->get(0);
145+
auto s2 = list->get(1);
146+
auto s3 = list->get(2);
147+
auto s4 = list->get(3);
148+
level *= 100;
149+
((MotorModule*) s1)->motorControl->setLevel(level);
150+
((MotorModule*) s2)->motorControl->setLevel(level);
151+
((MotorModule*) s3)->motorControl->setLevel(level);
152+
((MotorModule*) s4)->motorControl->setLevel(level);
153+
});
154+
HomeGenie::getInstance()->addAPIHandler(steerControl);
155+
156+
// initialize motors
157+
auto arm = std::vector<String>{"S1", "S2", "S3", "S4"};
158+
for (const auto &addr: arm) {
159+
auto sm = motorHandler->getModule(IO::IOEventDomains::Automation_Components, addr.c_str());
160+
if (sm) {
161+
// "10" = max position seeking speed (1 - 10)
162+
((MotorModule*) sm)->setProperty(Options::Motor_Servo_AngleSpeed, "10");
163+
// position to 50 (rest position)
164+
((MotorModule*) sm)->motorControl->setLevel(50); // (0-100)
165+
}
166+
}
167+
}
168+
}
169+
170+
isRoboticArm = Config::getSetting("robotc-arm").equals("true");
171+
if (isRoboticArm) {
172+
// If this device is for the Robotic Arm only it will be using modules "S1" to "S4".
173+
// If this device is for RC-CAR with Robotic Arm, arm modules will be "S5" to "S8".
174+
// Configure Robotic Arm initial position to 0.5
175+
auto arm = !is4wdRcCar ? std::vector<String>{"S1", "S2", "S3", "S4"} : std::vector<String>{"S5", "S6", "S7", "S8"};
176+
for (const auto &addr: arm) {
177+
auto sm = motorHandler->getModule(IO::IOEventDomains::Automation_Components, addr.c_str());
178+
if (sm) {
179+
// max speed to seek initial position
180+
((MotorModule *) sm)->setProperty(Options::Motor_Servo_AngleSpeed, "10");
181+
// position to 50 (rest position)
182+
((MotorModule *) sm)->motorControl->setLevel(50); // (0-100)
183+
}
184+
}
185+
}
186+
61187
}
62188

63189
void loop()
64190
{
65191
homeGenie->loop();
192+
193+
if (isRoboticArm && !roboArmInitialized && millis() > 3000) {
194+
roboArmInitialized = true;
195+
auto arm = !is4wdRcCar ? std::vector<String>{"S1", "S2", "S3", "S4"} : std::vector<String>{"S5", "S6", "S7", "S8"};
196+
for (const auto &addr: arm) {
197+
auto sm = motorHandler->getModule(IO::IOEventDomains::Automation_Components, addr.c_str());
198+
if (sm) {
199+
// set slower speed for normal operations (smoother movements)
200+
sm->setProperty(Options::Motor_Servo_AngleSpeed, "5");
201+
}
202+
}
203+
}
66204
}

0 commit comments

Comments
 (0)