Skip to content
Permalink
Browse files

Constrain joystick rate of change between samples.

  • Loading branch information...
Roger-random committed May 16, 2019
1 parent eaf3f61 commit ee63e94670e133e9258bf3e88774ded402e9b854
Showing with 46 additions and 5 deletions.
  1. +38 −4 arduino_sawppy/joydrive.cpp
  2. +7 −0 arduino_sawppy/joydrive.h
  3. +1 −1 arduino_sawppy/lewansoul.cpp
@@ -15,24 +15,36 @@ JoyDrive::JoyDrive(int steeringPin, int velocityPin)
{
_steeringPin = steeringPin;
_velocityPin = velocityPin;
_previousSteering = 0;
_previousVelocity = 0;
}

int JoyDrive::getSteering()
{
// analogRead returns between 0 and 1023
int vRaw = analogRead(_steeringPin);
int steeringRaw = analogRead(_steeringPin);
int steeringNormalized = normalized(steeringRaw, STEER_MID, INVERT_STEERING);
int steeringConstrained = constrained(steeringNormalized, _previousSteering, MAX_DELTA_STEERING);

return normalized(vRaw, STEER_MID, INVERT_STEERING);
_previousSteering = steeringConstrained;

return steeringConstrained;
}

int JoyDrive::getVelocity()
{
// analogRead returns between 0 and 1023
int vRaw = analogRead(_velocityPin);
int velocityRaw = analogRead(_velocityPin);
int velocityNormalized = normalized(velocityRaw, VEL_MID, INVERT_VELOCITY);
int velocityConstrained = constrained(velocityNormalized, _previousVelocity, MAX_DELTA_VELOCITY);

_previousVelocity = velocityConstrained;

return normalized(vRaw, VEL_MID, INVERT_VELOCITY);
return velocityConstrained;
}

// The normalized() function accepts a value in the full range of ADC (0-1023) and
// turns it into a range from -100 to 100 with 0 at the given midpoint.
int JoyDrive::normalized(int raw, int midpoint, bool invert)
{
float range = ADC_MID - DEAD_ZONE;
@@ -76,3 +88,25 @@ int JoyDrive::normalized(int raw, int midpoint, bool invert)

return normalized;
}

// The constrained() function accepts a normalized value and returns a
// value that is not more than maxDelta away from previous value.
int JoyDrive::constrained(int normalized, int previous, int maxDelta)
{
int constrained = 0;

if (normalized > previous + maxDelta)
{
constrained = previous + maxDelta;
}
else if (normalized < previous - maxDelta)
{
constrained = previous - maxDelta;
}
else
{
constrained = normalized;
}

return constrained;
}
@@ -22,6 +22,9 @@
#define INVERT_STEERING false
#define INVERT_VELOCITY false

#define MAX_DELTA_STEERING 10
#define MAX_DELTA_VELOCITY 15

class JoyDrive
{
public:
@@ -32,8 +35,12 @@ class JoyDrive

private:
int normalized(int raw, int midpoint, bool invert);
int constrained(int normalized, int previous, int maxDelta);

int _steeringPin;
int _velocityPin;

int _previousSteering;
int _previousVelocity;
};
#endif // joydrive_h
@@ -364,7 +364,7 @@ void LewanSoul::moveTo(int servoId, float destination)

// Output to LewanSoul API is from 0 to 1000, with 500 as center.
// Interpreted as movement from -120 to 120 degrees.
LobotSerialServoMove(Serial, servoId, (destination * (500.0/120.0))+500, 100);
LobotSerialServoMove(Serial, servoId, (destination * (500.0/120.0))+500, 200);
}

void LewanSoul::spinAt(int servoId, float velocity)

0 comments on commit ee63e94

Please sign in to comment.
You can’t perform that action at this time.