8000 add motor control · rjwats/esp8266-react@d25e0b4 · GitHub
[go: up one dir, main page]
Skip to content

Commit d25e0b4

Browse files
rjwatsrjwats
committed
add motor control
1 parent 188274d commit d25e0b4

File tree

2 files changed

+131
-16
lines changed

2 files changed

+131
-16
lines changed

‎src/RobotStateService.cpp

Lines changed: 88 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,14 +19,26 @@ RobotStateService::RobotStateService(AsyncWebServer* server, SecurityManager* se
1919
}
2020

2121
void RobotStateService::begin() {
22-
camXServo.attach(CAM_X_SERVO_PIN, 2);
23-
camYServo.attach(CAM_Y_SERVO_PIN, 3);
24-
clawServo.attach(CLAW_SERVO_PIN, 4);
22+
camXServo.attach(CAM_X_SERVO_PIN, CAM_X_SERVO_CHANNEL);
23+
camYServo.attach(CAM_Y_SERVO_PIN, CAM_Y_SERVO_CHANNEL);
24+
clawServo.attach(CLAW_SERVO_PIN, CLAW_SERVO_CHANNEL);
2525

2626
// configure the default state
2727
_state.camX = DEFAULT_CAM_X;
2828
_state.camY = DEFAULT_CAM_Y;
2929
_state.claw = DEFAULT_CLAW;
30+
_state.driveX = DEFAULT_DRIVE_X;
31+
_state.driveY = DEFAULT_DRIVE_Y;
32+
33+
// configure the PWM pins for both motors
34+
ledcSetup(LEFT_MOTOR_PWM_CHANNEL, MOTOR_PWM_FREQUENCY, MOTOR_PWM_RESOLUTION);
35+
ledcAttachPin(LEFT_MOTOR_PWM_PIN, LEFT_MOTOR_PWM_CHANNEL);
36+
ledcSetup(RIGHT_MOTOR_PWM_CHANNEL, MOTOR_PWM_FREQUENCY, MOTOR_PWM_RESOLUTION);
37+
ledcAttachPin(RIGHT_MOTOR_PWM_PIN, RIGHT_MOTOR_PWM_CHANNEL);
38+
39+
// configure direction pins for both motors
40+
pinMode(LEFT_MOTOR_DIRECTION_PIN, OUTPUT);
41+
pinMode(RIGHT_MOTOR_DIRECTION_PIN, OUTPUT);
3042

3143
// apply the default state
3244
onConfigUpdated();
@@ -36,4 +48,77 @@ void RobotStateService::onConfigUpdated() {
3648
camXServo.write(_state.camX);
3749
camYServo.write(_state.camY);
3850
clawServo.write(_state.claw);
51+
52+
// calculate and convert motor outputs to PWM outputs for h-bridge
53+
struct MotorOutputs outputs = calculateMotorOutputs(_state.driveX, _state.driveY);
54+
applyMotorOutput(outputs.left, LEFT_MOTOR_DIRECTION_PIN, LEFT_MOTOR_PWM_CHANNEL);
55+
applyMotorOutput(outputs.right, RIGHT_MOTOR_DIRECTION_PIN, RIGHT_MOTOR_PWM_CHANNEL);
3956
}
57+
58+
void RobotStateService::applyMotorOutput(int32_t output, uint8_t directionPin, uint8_t pwmChan) {
59+
bool forward = output >= 0;
60+
output = abs(output);
61+
output = output > DEAD_ZONE ? output : 0;
62+
if (output > 0) {
63+
output = map(output, 0, 1023, 600, 1023);
64+
}
65+
if (forward) {
66+
digitalWrite(directionPin, HIGH);
67+
ledcWrite(pwmChan, 1023 - output);
68+
} else {
69+
digitalWrite(directionPin, LOW);
70+
ledcWrite(pwmChan, output);
71+
}
72+
}
73+
74+
// Wonderful code below borrowed from https://www.impulseadventure.com/elec/robot-differential-steering.html
75+
struct MotorOutputs RobotStateService::calculateMotorOutputs(int32_t nJoyX, int32_t nJoyY) {
76+
// OUTPUTS
77+
int32_t nMotMixL; // Motor (left) mixed output (-1023..+1023)
78+
int32_t nMotMixR; // Motor (right) mixed output (-1023..+1023)
79+
80+
// CONFIG
81+
// - fPivYLimt : The threshold at which the pivot action starts
82+
// This threshold is measured in units on the Y-axis
83+
// away from the X-axis (Y=0). A greater value will assign
84+
// more of the joystick's range to pivot actions.
85+
// Allowable range: (0..+1023)
86+
float fPivYLimit = 255.0;
87+
88+
// TEMP VARIABLES
89+
float nMotPremixL; // Motor (left) premixed output (-1023..+1023)
90+
float nMotPremixR; // Motor (right) premixed output (-1023..+1023)
91+
int32_t nPivSpeed; // Pivot Speed (-1023..+1023)
92+
float fPivScale; // Balance scale b/w drive and pivot ( 0..1 )
93+
94+
// Calculate Drive Turn output due to Joystick X input
95+
if (nJoyY >= 0) {
96+
// Forward
97+
nMotPremixL = (nJoyX >= 0) ? 1023.0 : (1023.0 + nJoyX);
98+
nMotPremixR = (nJoyX >= 0) ? (1023.0 - nJoyX) : 1023.0;
99+
} else {
100+
// Reverse
101+
nMotPremixL = (nJoyX >= 0) ? (1023.0 - nJoyX) : 1023.0;
102+
nMotPremixR = (nJoyX >= 0) ? 1023.0 : (1023.0 + nJoyX);
103+
}
104+
105+
// Scale Drive output due to Joystick Y input (throttle)
106+
nMotPremixL = nMotPremixL * nJoyY / 1023.0;
107+
nMotPremixR = nMotPremixR * nJoyY / 1023.0;
108+
109+
// Now calculate pivot amount
110+
// - Strength of pivot (nPivSpeed) based on Joystick X input
111+
// - Blending of pivot vs drive (fPivScale) based on Joystick Y input
112+
nPivSpeed = nJoyX;
113+
fPivScale = (abs(nJoyY) > fPivYLimit) ? 0.0 : (1.0 - abs(nJoyY) / fPivYLimit);
114+
115+
// Calculate final mix of Drive and Pivot
116+
nMotMixL = (1.0 - fPivScale) * nMotPremixL + fPivScale * (nPivSpeed);
117+
nMotMixR = (1.0 - fPivScale) * nMotPremixR + fPivScale * (-nPivSpeed);
118+
119+
// return as a struct
120+
struct MotorOutputs outputs {
121+
nMotMixL, nMotMixR
122+
};
123+
return outputs;
124+
}

‎src/RobotStateService.h

Lines changed: 43 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -8,42 +8,70 @@
88

99
#define LED_PIN 2
1010

11-
#define DEFAULT_CAM_Y 90
12-
#define DEFAULT_CAM_X 135
11+
#define DEFAULT_CAM_Y 135
12+
#define DEFAULT_CAM_X 90
1313
#define DEFAULT_CLAW 90
14+
#define DEFAULT_DRIVE_X 0
15+
#define DEFAULT_DRIVE_Y 0
1416

1517
#define CAM_X_SERVO_PIN 2
16-
#define CAM_Y_SERVO_PIN 16
17-
#define CLAW_SERVO_PIN 4
18+
#define CAM_X_SERVO_CHANNEL 10
19+
20+
#define CAM_Y_SERVO_PIN 4
21+
#define CAM_Y_SERVO_CHANNEL 11
22+
23+
#define CLAW_SERVO_PIN 16
24+
#define CLAW_SERVO_CHANNEL 12
25+
26+
#define LEFT_MOTOR_PWM_PIN 12
27+
#define LEFT_MOTOR_DIRECTION_PIN 13
28+
#define LEFT_MOTOR_PWM_CHANNEL 5
29+
30+
#define RIGHT_MOTOR_PWM_PIN 15
31+
#define RIGHT_MOTOR_DIRECTION_PIN 14
32+
#define RIGHT_MOTOR_PWM_CHANNEL 6
33+
34+
#define MOTOR_PWM_RESOLUTION 10
35+
#define MOTOR_PWM_FREQUENCY 4096
36+
37+
#define DEAD_ZONE 20
1838

1939
#define ROBOT_SETTINGS_ENDPOINT_PATH "/rest/robotState"
2040
#define ROBOT_SETTINGS_SOCKET_PATH "/ws/robotState"
2141

42+
struct MotorOutputs {
43+
int32_t left;
44+
int32_t right;
45+
};
46+
2247
class RobotState {
2348
public:
2449
uint8_t camX;
2550
uint8_t camY;
2651
uint8_t claw;
52+
int32_t driveX;
53+
int32_t driveY;
2754

2855
static void read(RobotState& settings, JsonObject& root) {
2956
root["cam_x"] = settings.camX;
3057
root["cam_y"] = settings.camY;
3158
root["claw"] = settings.claw;
59+
root["drive_x"] = settings.driveX;
60+
root["drive_y"] = settings.driveY;
3261
}
3362

3463
static StateUpdateResult update(JsonObject& root, RobotState& robotState) {
35-
robotState.camX = readBoundedByte(root, "cam_x", 0, 180, DEFAULT_CAM_X);
36-
robotState.camY = readBoundedByte(root, "cam_y", 0, 180, DEFAULT_CAM_Y);
37-
robotState.claw = readBoundedByte(root, "claw", 90, 180, DEFAULT_CLAW);
64+
robotState.camX = readBounded(root, "cam_x", 0, 180, DEFAULT_CAM_X);
65+
robotState.camY = readBounded(root, "cam_y", 0, 180, DEFAULT_CAM_Y);
66+
robotState.claw = readBounded(root, "claw", 90, 180, DEFAULT_CLAW);
67+
robotState.driveX = readBounded(root, "drive_x", -1023, 1023, DEFAULT_DRIVE_X);
68+
robotState.driveY = readBounded(root, "drive_y", -1023, 1023, DEFAULT_DRIVE_Y);
3869
return StateUpdateResult::CHANGED;
3970
}
4071

41-
static uint8_t readBoundedByte(JsonObject& root,
42-
String prop,
43-
uint8_t minValue,
44-
uint8_t maxValue,
45-
uint8_t defaultValue) {
46-
uint8_t value = root[prop] | defaultValue;
72+
template <typename N>
73+
static N readBounded(JsonObject& root, String prop, N minValue, N maxValue, N defaultValue) {
74+
N value = root[prop] | defaultValue;
4775
return max(min(value, maxValue), minValue);
4876
}
4977
};
@@ -61,6 +89,8 @@ class RobotStateService : public StatefulService<RobotState> {
6189
Servo clawServo;
6290

6391
void onConfigUpdated();
92+
struct MotorOutputs calculateMotorOutputs(int32_t nJoyX, int32_t nJoyY);
93+
void applyMotorOutput(int32_t output, uint8_t directionPin, uint8_t pwmChan);
6494
};
6595

6696
#endif

0 commit comments

Comments
 (0)
0