From 2b5f3c85e06dd948ff992cbe2a7995d2a6d05229 Mon Sep 17 00:00:00 2001 From: Greg Date: Sun, 9 Aug 2020 22:21:20 -0400 Subject: [PATCH 01/17] Changed startSetpoint to current position --- src/Motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 4b23df64..2d0c04d0 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -133,7 +133,7 @@ void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, if (newSetpoint == Setpoint && msTimeDuration == duration && this->mode == mode) return; - startSetpoint = Setpoint; + startSetpoint = encoder->getCount(); endSetpoint = newSetpoint; startTime = millis(); duration = msTimeDuration; From 720a481449cad0150147e83e0241dc7ae0c65574 Mon Sep 17 00:00:00 2001 From: Greg Date: Thu, 13 Aug 2020 21:53:12 -0400 Subject: [PATCH 02/17] basic pid --- .gitignore | 2 ++ src/Motor.cpp | 13 +++++++------ src/Motor.h | 4 ++-- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index 259148fa..4e65aaef 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,5 @@ *.exe *.out *.app +.vscode/c_cpp_properties.json +.vscode/settings.json diff --git a/src/Motor.cpp b/src/Motor.cpp index 2d0c04d0..3e6abc80 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -271,7 +271,7 @@ void Motor::loop() { if (closedLoopControl) { //portEXIT_CRITICAL(&mmux); if (mode == VELOCITY_MODE) { - if(abs(currentEffort)<0.95)// stall detection + if(fabs(currentEffort)<0.95)// stall detection Setpoint += milisecondPosIncrementForVelocity; } else { unitDuration = getInterpolationUnitIncrement(); @@ -287,15 +287,15 @@ void Motor::loop() { } float controlErr = Setpoint - nowEncoder; // shrink old values out of the sum - runntingITerm = runntingITerm * ((I_TERM_SIZE - 1.0) / I_TERM_SIZE); + //runntingITerm = runntingITerm * ((I_TERM_SIZE - 1.0) / I_TERM_SIZE); // running sum of error - runntingITerm += controlErr; + if(fabs(currentEffort)<0.95) runningITerm += controlErr; if(getInterpolationUnitIncrement()<1){ // no i term during interpolation - runntingITerm=0; + //runntingITerm=0; } - currentEffort = controlErr * kP + ((runntingITerm / I_TERM_SIZE) * kI); + currentEffort = controlErr * kP + runningITerm * kI; //portEXIT_CRITICAL(&mmux); } @@ -319,7 +319,7 @@ void Motor::setGains(float p, float i, float d) { kP = p; kI = i; kD = d; - runntingITerm = 0; + runningITerm = 0; //portEXIT_CRITICAL(&mmux); } @@ -369,6 +369,7 @@ void Motor::SetEffort(float effort) { //portENTER_CRITICAL(&mmux); closedLoopControl = false; currentEffort = effort; + pwm->writeScaled(abs(effort)); //portEXIT_CRITICAL(&mmux); } /* diff --git a/src/Motor.h b/src/Motor.h index a5ba8492..02f567ce 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -74,7 +74,7 @@ class Motor { /** * PID controller integral constant */ - float kI = 0.06; + float kI = 0.00001; /** * PID controller derivitive constant */ @@ -82,7 +82,7 @@ class Motor { /** * a variable to store the running avarage for the integral term */ - float runntingITerm = 0; + float runningITerm = 0; /* * effort of the motor * @param a value from -1 to 1 representing effort From 3cc0609f09ceb17a77902000add66e5cb8a4e5c5 Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 15 Aug 2020 00:13:44 -0400 Subject: [PATCH 03/17] Rebuilt motor control on speed --- src/Motor.cpp | 323 +++++++++++++++++++++++++------------------------- src/Motor.h | 118 +++++++++++------- 2 files changed, 233 insertions(+), 208 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 3e6abc80..0a0bb741 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -10,7 +10,11 @@ bool Motor::timersAllocated = false; Motor * Motor::list[MAX_POSSIBLE_MOTORS] = { NULL, }; static TaskHandle_t complexHandlerTask; -//portMUX_TYPE mmux = portMUX_INITIALIZER_UNLOCKED; + + +float TICKS_PER_DEGREE = 1.0/TICKS_TO_DEGREES; + +const uint32_t LOOP_PERIOD_MS = 10; float myFmapBounded(float x, float in_min, float in_max, float out_min, float out_max) { @@ -24,60 +28,61 @@ float myFmapBounded(float x, float in_min, float in_max, float out_min, * getInterpolationUnitIncrement A unit vector from * 0 to 1 representing the state of the interpolation. */ -float Motor::getInterpolationUnitIncrement() { - float interpElapsed = (float) (millis() - startTime); - if (interpElapsed < duration && duration > 0) { - // linear elapsed duration - unitDuration = interpElapsed / duration; - if (mode == SINUSOIDAL_INTERPOLATION) { - // sinusoidal ramp up and ramp down - float sinPortion = (cos(-PI * unitDuration) / 2) + 0.5; - unitDuration = 1 - sinPortion; - } - if(mode==BEZIER){ - if(unitDuration>0 &&unitDuration<1){ - float t=unitDuration; - float P0=0; - float P1=BEZIER_P0; - float P2=BEZIER_P1; - float P3=1; - unitDuration= pow((1-t),3) *P0 + - 3*t*pow((1-t),2)*P1 + - 3*pow(t,2)*(1-t)*P2 + - pow(t,3)*P3; - } - } - if(mode == TRAPEZOIDAL){ - float lengthOfLinearMode = duration-(TRAPEZOIDAL_time*2); - float unitLienear = lengthOfLinearMode/duration; - float unitRamp = ((float)TRAPEZOIDAL_time)/duration; - float unitStartRampDown = unitLienear+unitRamp; - if(unitDurationunitRamp&&unitDuration 0) { +// // linear elapsed duration +// unitDuration = interpElapsed / duration; +// if (mode == SINUSOIDAL_INTERPOLATION) { +// // sinusoidal ramp up and ramp down +// float sinPortion = (cos(-PI * unitDuration) / 2) + 0.5; +// unitDuration = 1 - sinPortion; +// } +// if(mode==BEZIER){ +// if(unitDuration>0 &&unitDuration<1){ +// float t=unitDuration; +// float P0=0; +// float P1=BEZIER_P0; +// float P2=BEZIER_P1; +// float P3=1; +// unitDuration= pow((1-t),3) *P0 + +// 3*t*pow((1-t),2)*P1 + +// 3*pow(t,2)*(1-t)*P2 + +// pow(t,3)*P3; +// } +// } +// if(mode == TRAPEZOIDAL){ +// float lengthOfLinearMode = duration-(TRAPEZOIDAL_time*2); +// float unitLienear = lengthOfLinearMode/duration; +// float unitRamp = ((float)TRAPEZOIDAL_time)/duration; +// float unitStartRampDown = unitLienear+unitRamp; +// if(unitDurationunitRamp&&unitDurationunitStartRampDown){ +// float increment=(unitDuration-unitStartRampDown)/(unitRamp*2)+0.5; +// float sinPortion = 0.5-((cos(-PI *increment) / 2) + 0.5); +// unitDuration = (sinPortion*2)*unitRamp+unitStartRampDown; +// } +// } +// return unitDuration; +// } +// return 1; +// } - } - else if(unitDuration>unitStartRampDown){ - float increment=(unitDuration-unitStartRampDown)/(unitRamp*2)+0.5; - float sinPortion = 0.5-((cos(-PI *increment) / 2) + 0.5); - unitDuration = (sinPortion*2)*unitRamp+unitStartRampDown; - } - } - return unitDuration; - } - return 1; -} void onMotorTimer(void *param) { Serial.println("Starting the PID loop thread"); TickType_t xLastWakeTime; - const TickType_t xFrequency = 1; + const TickType_t xFrequency = LOOP_PERIOD_MS; xLastWakeTime = xTaskGetTickCount(); while (1) { vTaskDelayUntil(&xLastWakeTime, xFrequency); @@ -119,30 +124,30 @@ Motor::~Motor() { delete (pwm); } -/** - * SetSetpoint in degrees with time - * Set the setpoint for the motor in degrees - * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param msTimeDuration the number of miliseconds to get from current position to the new setpoint - */ -void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, - interpolateMode mode) { - float newSetpoint = newTargetInDegrees / TICKS_TO_DEGREES; - //portENTER_CRITICAL(&mmux); - closedLoopControl = true; - if (newSetpoint == Setpoint && msTimeDuration == duration - && this->mode == mode) - return; - startSetpoint = encoder->getCount(); - endSetpoint = newSetpoint; - startTime = millis(); - duration = msTimeDuration; - this->mode = mode; - if (msTimeDuration < 1) { - Setpoint = newSetpoint; - } - //portEXIT_CRITICAL(&mmux); -} +// /** +// * SetSetpoint in degrees with time +// * Set the setpoint for the motor in degrees +// * @param newTargetInDegrees the new setpoint for the closed loop controller +// * @param msTimeDuration the number of miliseconds to get from current position to the new setpoint +// */ +// void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, +// interpolateMode mode) { +// float newSetpoint = newTargetInDegrees / TICKS_TO_DEGREES; +// //portENTER_CRITICAL(&mmux); +// closedLoopControl = true; +// if (newSetpoint == Setpoint && msTimeDuration == duration +// && this->mode == mode) +// return; +// startSetpoint = encoder->getCount(); +// endSetpoint = newSetpoint; +// startTime = millis(); +// duration = msTimeDuration; +// this->mode = mode; +// if (msTimeDuration < 1) { +// Setpoint = newSetpoint; +// } +// //portEXIT_CRITICAL(&mmux); +// } /** * MoveTo in degrees with speed @@ -153,45 +158,52 @@ void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, */ void Motor::MoveTo(float newTargetInDegrees, float speedDegPerSec) { - SetSetpointWithTime(newTargetInDegrees, fabs(newTargetInDegrees/speedDegPerSec) * 1000.0, SINUSOIDAL_INTERPOLATION); + //SetSetpointWithTime(newTargetInDegrees, fabs(newTargetInDegrees/speedDegPerSec) * 1000.0, SINUSOIDAL_INTERPOLATION); } - float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) { - float newSetPoint = getCurrentDegrees() + deltaTargetInDegrees; - SetSetpointWithTime(newSetPoint, - fabs(deltaTargetInDegrees / speedDegPerSec) * 1000.0, - SINUSOIDAL_INTERPOLATION); - return newSetPoint; -} + currTrajectory.startPos = getCurrentDegrees() * TICKS_PER_DEGREE; -/** - * \brief wait for the motor to arrive at a setpoint - * - * @note this is a blocking function, it will block code for multiple seconds until the motor arrives - * at its given setpoint - */ -void Motor::blockUntilMoveIsDone(){ - float distanceToGo; - // First wait for the interpolation to finish - while(getInterpolationUnitIncrement()<1){ - delay(10); - Serial.println(" Interpolation "+String (getInterpolationUnitIncrement())); - } - do - { - delay(10); - distanceToGo=fabs((Setpoint*TICKS_TO_DEGREES) - getCurrentDegrees()); - Serial.println("Remaining: "+String(distanceToGo)); - }while (distanceToGo>TICKS_TO_DEGREES );// get within 1 tick - // wait for the velocity to be below 10deg/sec - // 5deg/sec is lower bound of detection - while (fabs(getDegreesPerSecond()) > 10) { - Serial.println("Speed: "+String(getDegreesPerSecond())); - delay(10); - } + float targetPosDeg = getCurrentDegrees() + deltaTargetInDegrees; + currTrajectory.targetPos = targetPosDeg * TICKS_PER_DEGREE; + + if(deltaTargetInDegrees < 0 && speedDegPerSec > 0) speedDegPerSec *= -1; + if(deltaTargetInDegrees > 0 && speedDegPerSec < 0) speedDegPerSec *= -1; + + SetDelta(speedDegPerSec); + mode = LINEAR_INTERPOLATION; + + return targetPosDeg; } + +// /** +// * \brief wait for the motor to arrive at a setpoint +// * +// * @note this is a blocking function, it will block code for multiple seconds until the motor arrives +// * at its given setpoint +// */ +// void Motor::blockUntilMoveIsDone() +// { +// float distanceToGo; +// // First wait for the interpolation to finish +// while(getInterpolationUnitIncrement()<1){ +// delay(10); +// Serial.println(" Interpolation "+String (getInterpolationUnitIncrement())); +// } +// do +// { +// delay(10); +// distanceToGo=fabs((Setpoint*TICKS_TO_DEGREES) - getCurrentDegrees()); +// Serial.println("Remaining: "+String(distanceToGo)); +// }while (distanceToGo>TICKS_TO_DEGREES );// get within 1 tick +// // wait for the velocity to be below 10deg/sec +// // 5deg/sec is lower bound of detection +// while (fabs(getDegreesPerSecond()) > 10) { +// Serial.println("Speed: "+String(getDegreesPerSecond())); +// delay(10); +// } +// } /** * MoveFor a relative amount in degrees with speed * Set the setpoint for the motor in degrees and the speed you want to get there @@ -204,7 +216,7 @@ void Motor::blockUntilMoveIsDone(){ void Motor::MoveFor(float deltaTargetInDegrees, float speedDegPerSec) { StartMoveFor(deltaTargetInDegrees, speedDegPerSec); - blockUntilMoveIsDone(); + while(currTrajectory.FractionComplete() < 1.0) {Serial.println(currTrajectory.FractionComplete());} } /** @@ -224,22 +236,23 @@ void Motor::MoveFor(float deltaTargetInDegrees, float speedDegPerSec) * * @param newDegreesPerSecond the new speed in degrees per second */ -void Motor::SetSpeed(float newDegreesPerSecond) { - if (abs(newDegreesPerSecond) < 0.1) { - SetSetpoint(getCurrentDegrees()); -// Serial.println("Stopping"); - return; - } - milisecondPosIncrementForVelocity = (-newDegreesPerSecond - * (((float) -1.0) / 1000.0)) / TICKS_TO_DEGREES; -// Serial.println("Setting Speed "+String(newDegreesPerSecond)+ -// " increment "+String(milisecondPosIncrementForVelocity)+ -// " scale "+String(TICKS_TO_DEGREES) -// +" Setpoint "+String(Setpoint*TICKS_TO_DEGREES) -// ); - //Setpoint = nowEncoder; +void Motor::SetSpeed(float speedDegPerSec) +{ + SetDelta(speedDegPerSec); mode = VELOCITY_MODE; +} + +float Motor::SetDelta(float speedDegPerSec) +{ + deltaTargetTicks = speedDegPerSec * TICKS_PER_DEGREE * LOOP_PERIOD_MS / 1000.0; + Serial.println("Setting Speed "+String(speedDegPerSec)+ + " increment "+String(deltaTargetTicks) + ); + + Setpoint = nowEncoder; closedLoopControl = true; + + return deltaTargetTicks; } /** @@ -266,40 +279,37 @@ void Motor::SetSpeed(float newDegreesPerSecond, long miliseconds) { * this method is called by the timer to run the PID control of the motors and ensure strict timing * */ -void Motor::loop() { +void Motor::loop() +{ nowEncoder = encoder->getCount(); - if (closedLoopControl) { - //portEXIT_CRITICAL(&mmux); - if (mode == VELOCITY_MODE) { - if(fabs(currentEffort)<0.95)// stall detection - Setpoint += milisecondPosIncrementForVelocity; - } else { - unitDuration = getInterpolationUnitIncrement(); - if (unitDuration < 1) { - float setpointDiff = endSetpoint - startSetpoint; - float newSetpoint = startSetpoint - + (setpointDiff * unitDuration); - Setpoint = newSetpoint; - } else { - // If there is no interpoation to perform, set the setpoint to the end state - Setpoint = endSetpoint; - } + currTrajectory.currPos = nowEncoder; + + if (closedLoopControl) + { + if (mode == VELOCITY_MODE) + { + if(fabs(currentEffort)<0.95)// stall detection -- could be less, could be better + Setpoint += deltaTargetTicks; + } + + else + { + if(currTrajectory.FractionComplete() >= 1.0) + { + Setpoint = currTrajectory.targetPos; + } + else if(fabs(currentEffort)<0.5)// stall detection -- could be less, could be better + Setpoint += deltaTargetTicks; } + float controlErr = Setpoint - nowEncoder; - // shrink old values out of the sum - //runntingITerm = runntingITerm * ((I_TERM_SIZE - 1.0) / I_TERM_SIZE); - // running sum of error - if(fabs(currentEffort)<0.95) runningITerm += controlErr; - if(getInterpolationUnitIncrement()<1){ - // no i term during interpolation - //runntingITerm=0; - } - currentEffort = controlErr * kP + runningITerm * kI; + if(fabs(currentEffort)<0.5) runningITerm += controlErr; - //portEXIT_CRITICAL(&mmux); + currentEffort = controlErr * kP + runningITerm * kI; } + //this needs redoing interruptCountForVelocity++; if (interruptCountForVelocity == 50) { interruptCountForVelocity = 0; @@ -309,7 +319,6 @@ void Motor::loop() { } // invert the effort so that the set speed and set effort match SetEffortLocal(currentEffort); - } /** * PID gains for the PID controller @@ -361,17 +370,7 @@ void Motor::attach(int MotorPWMPin, int MotorDirectionPin, int EncoderA, * 1 is full speed clockwise * -1 is full speed counter clockwise */ -void Motor::SetEffort(float effort) { - if (effort > 1) - effort = 1; - if (effort < -1) - effort = -1; - //portENTER_CRITICAL(&mmux); - closedLoopControl = false; - currentEffort = effort; - pwm->writeScaled(abs(effort)); - //portEXIT_CRITICAL(&mmux); -} +void Motor::SetEffort(float effort) { closedLoopControl = false; SetEffortLocal(effort); } /* * effort of the motor * @return a value from -1 to 1 representing effort diff --git a/src/Motor.h b/src/Motor.h index 02f567ce..7252d532 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -15,10 +15,26 @@ #define GEAR_BOX_RATIO 120.0f #define QUADRATUE_MULTIPLYER 1.0f #define TICKS_TO_DEGREES (QUADRATUE_MULTIPLYER/(ENCODER_CPR*GEAR_BOX_RATIO/360.0)) + #define I_TERM_SIZE 120.0f enum interpolateMode { LINEAR_INTERPOLATION, SINUSOIDAL_INTERPOLATION, VELOCITY_MODE, BEZIER, TRAPEZOIDAL }; + +struct Trajectory +{ + //all in ticks + int32_t startPos = 0; + int32_t targetPos = 0; + int32_t currPos = 0; + + //int32_t targetSpeed = 0; + + float FractionComplete(void) { return (float)(currPos - startPos) / (float)(targetPos - startPos); } +}; + + + /** \brief A PID Motor class using FreeRTOS threads, ESP32Encoder and ESP32PWM * * This Motor class is intended to be used by RBE 1001 in the WPI Robotics Department. @@ -70,11 +86,11 @@ class Motor { /** * PID controller proportional constant */ - float kP = 0.05; + float kP = 0.01; /** * PID controller integral constant */ - float kI = 0.00001; + float kI = 0.00004; /** * PID controller derivitive constant */ @@ -104,23 +120,23 @@ class Motor { /** * PID controller Interpolation duration in miliseconds */ - float duration = 0; +// float duration = 0; /** * PID controller Interpolation time in miliseconds that the interplation began */ - float startTime = 0; +// float startTime = 0; /** * PID controller Interpolation setpoint for the interpolation to arrive at */ - float endSetpoint = 0; +// float endSetpoint = 0; /** * PID controller Interpolation setpoint at the start of interpolation */ - float startSetpoint = 0; +// float startSetpoint = 0; /** * Duration of the interpolation mode, 1 equals done, 0 starting */ - float unitDuration = 1; +// float unitDuration = 1; /** * Current interpolation mode, linear, sinusoidal or velocity */ @@ -130,24 +146,31 @@ class Motor { * when using Red Queen mode for velocity interpolation, this is the amount of setpoint to add to the current setpoint * every milisecond to maintain a smooth velocity trajectory. */ - float milisecondPosIncrementForVelocity; +// float milisecondPosIncrementForVelocity; /** * \brief BEZIER Control Point 0 * * https://stackoverflow.com/a/43071667 */ - float BEZIER_P0=0.25; +// float BEZIER_P0=0.25; /** * \brief BEZIER Control Point 1 * * https://stackoverflow.com/a/43071667 */ - float BEZIER_P1=0.75; +// float BEZIER_P1=0.75; /** * \brief the amount of time to ramp up and ramp down the speed */ - float TRAPEZOIDAL_time=0; +// float TRAPEZOIDAL_time=0; + + float deltaTargetTicks = 0; //used to set constant trajectory + float targetTrajTicks = 0; //target for tracking trajectory, in ticks + + float targetPosDeg = 0; //target position for a motion, in degrees + + Trajectory currTrajectory; public: /** @@ -288,6 +311,9 @@ class Motor { * @param newDegreesPerSecond the new speed in degrees per second */ void SetSpeed(float newDegreesPerSecond); + + float SetDelta(float newDegreesPerSecond); + /** * SetSpeed in degrees with time * Set the setpoint for the motor in degrees @@ -334,10 +360,10 @@ class Motor { * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - */ - void SetSetpoint(float newTargetInDegrees) { - SetSetpointWithTime(newTargetInDegrees, 0, LINEAR_INTERPOLATION); - } + // */ + // void SetSetpoint(float newTargetInDegrees) { + // SetSetpointWithTime(newTargetInDegrees, 0, LINEAR_INTERPOLATION); + // } /** * SetSetpoint in degrees with time @@ -345,12 +371,12 @@ class Motor { * @param newTargetInDegrees the new setpoint for the closed loop controller * @param miliseconds the number of miliseconds to get from current position to the new setpoint * use linear interoplation - */ - void SetSetpointWithLinearInterpolation(float newTargetInDegrees, - long miliseconds) { - SetSetpointWithTime(newTargetInDegrees, miliseconds, - LINEAR_INTERPOLATION); - } + // */ + // void SetSetpointWithLinearInterpolation(float newTargetInDegrees, + // long miliseconds) { + // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // LINEAR_INTERPOLATION); + // } /** * SetSetpoint in degrees with time @@ -358,12 +384,12 @@ class Motor { * @param newTargetInDegrees the new setpoint for the closed loop controller * @param miliseconds the number of miliseconds to get from current position to the new setpoint * use sinusoidal interpolation - */ - void SetSetpointWithSinusoidalInterpolation(float newTargetInDegrees, - long miliseconds) { - SetSetpointWithTime(newTargetInDegrees, miliseconds, - SINUSOIDAL_INTERPOLATION); - } + // */ + // void SetSetpointWithSinusoidalInterpolation(float newTargetInDegrees, + // long miliseconds) { + // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // SINUSOIDAL_INTERPOLATION); + // } /** * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees @@ -372,14 +398,14 @@ class Motor { * @param Control_0 On a scale of 0 to 1, where should the first control point in the equation go * @param Control_1 On a scale of 0 to 1, where should the second control point in the equation go * use Bezier interpolation - */ - void SetSetpointWithBezierInterpolation(float newTargetInDegrees, - long miliseconds, float Control_0=0.5, float Control_1=1.0) { - BEZIER_P0=Control_0; - BEZIER_P1=Control_1; - SetSetpointWithTime(newTargetInDegrees, miliseconds, - BEZIER); - } + // */ + // void SetSetpointWithBezierInterpolation(float newTargetInDegrees, + // long miliseconds, float Control_0=0.5, float Control_1=1.0) { + // BEZIER_P0=Control_0; + // BEZIER_P1=Control_1; + // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // BEZIER); + // } /** * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees @@ -389,17 +415,17 @@ class Motor { * * * use sinusoidal interpolation - */ - void SetSetpointWithTrapezoidalInterpolation(float newTargetInDegrees, - long miliseconds, float trapazoidalTime) { - if(trapazoidalTime*2>miliseconds){ - SetSetpointWithSinusoidalInterpolation(newTargetInDegrees,miliseconds); - return; - } - TRAPEZOIDAL_time=trapazoidalTime; - SetSetpointWithTime(newTargetInDegrees, miliseconds, - TRAPEZOIDAL); - } + // */ + // void SetSetpointWithTrapezoidalInterpolation(float newTargetInDegrees, + // long miliseconds, float trapazoidalTime) { + // if(trapazoidalTime*2>miliseconds){ + // SetSetpointWithSinusoidalInterpolation(newTargetInDegrees,miliseconds); + // return; + // } + // TRAPEZOIDAL_time=trapazoidalTime; + // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // TRAPEZOIDAL); + // } /** * PID gains for the PID controller */ From d83b47ecbf4fb9cf73689ca4b160b0496b15634e Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 15 Aug 2020 07:19:27 -0400 Subject: [PATCH 04/17] Added StartMoveTo, cleaned up MoveFor --- src/Motor.cpp | 20 +++++++++++++------- src/Motor.h | 3 +++ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 0a0bb741..b92cdae3 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -158,18 +158,18 @@ Motor::~Motor() { */ void Motor::MoveTo(float newTargetInDegrees, float speedDegPerSec) { - //SetSetpointWithTime(newTargetInDegrees, fabs(newTargetInDegrees/speedDegPerSec) * 1000.0, SINUSOIDAL_INTERPOLATION); + StartMoveTo(newTargetInDegrees, speedDegPerSec); + while(currTrajectory.FractionComplete() < 1.0) {Serial.println(currTrajectory.FractionComplete());} } -float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) + +float Motor::StartMoveTo(float newTargetInDegrees, float speedDegPerSec) { currTrajectory.startPos = getCurrentDegrees() * TICKS_PER_DEGREE; + currTrajectory.targetPos = newTargetInDegrees * TICKS_PER_DEGREE; - float targetPosDeg = getCurrentDegrees() + deltaTargetInDegrees; - currTrajectory.targetPos = targetPosDeg * TICKS_PER_DEGREE; - - if(deltaTargetInDegrees < 0 && speedDegPerSec > 0) speedDegPerSec *= -1; - if(deltaTargetInDegrees > 0 && speedDegPerSec < 0) speedDegPerSec *= -1; + if(currTrajectory.targetPos > currTrajectory.startPos) speedDegPerSec = fabs(speedDegPerSec); + else speedDegPerSec = -fabs(speedDegPerSec); SetDelta(speedDegPerSec); mode = LINEAR_INTERPOLATION; @@ -177,6 +177,12 @@ float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) return targetPosDeg; } +float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) +{ + float targetPosDeg = getCurrentDegrees() + deltaTargetInDegrees; + return StartMoveTo(targetPosDeg, speedDegPerSec); +} + // /** // * \brief wait for the motor to arrive at a setpoint // * diff --git a/src/Motor.h b/src/Motor.h index 7252d532..311189ba 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -329,6 +329,9 @@ class Motor { * @param speedDegPerSec is the speed in degrees per second */ void MoveTo(float newTargetInDegrees, float speedDegPerSec); + + float StartMoveTo(float newTargetInDegrees, float speedDegPerSec); + /** * MoveFor a relative amount in degrees with speed * Set the setpoint for the motor in degrees and the speed you want to get there From 0bceadabface92d38be85f4815fc5c4c841c0ad7 Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 15 Aug 2020 07:24:53 -0400 Subject: [PATCH 05/17] cleaned up targetPos/removed duplicate data --- src/Motor.cpp | 4 +++- src/Motor.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index b92cdae3..0876af94 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -160,6 +160,8 @@ void Motor::MoveTo(float newTargetInDegrees, float speedDegPerSec) { StartMoveTo(newTargetInDegrees, speedDegPerSec); while(currTrajectory.FractionComplete() < 1.0) {Serial.println(currTrajectory.FractionComplete());} + + return; } @@ -174,7 +176,7 @@ float Motor::StartMoveTo(float newTargetInDegrees, float speedDegPerSec) SetDelta(speedDegPerSec); mode = LINEAR_INTERPOLATION; - return targetPosDeg; + return newTargetInDegrees; } float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) diff --git a/src/Motor.h b/src/Motor.h index 311189ba..ef81d0dd 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -168,7 +168,7 @@ class Motor { float deltaTargetTicks = 0; //used to set constant trajectory float targetTrajTicks = 0; //target for tracking trajectory, in ticks - float targetPosDeg = 0; //target position for a motion, in degrees + //float targetPosDeg = 0; //target position for a motion, in degrees Trajectory currTrajectory; From 396627d439b5dd34cc14808a9fce1400d248fbb8 Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 15 Aug 2020 08:00:41 -0400 Subject: [PATCH 06/17] A little re-org --- src/Motor.cpp | 23 ++++++++++++++--------- src/Motor.h | 13 +++++-------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 0876af94..0ee0249b 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -7,12 +7,16 @@ #include +#define ENCODER_CPR 12.0f +#define GEAR_BOX_RATIO 120.0f +#define QUADRATUE_MULTIPLYER 1.0f + bool Motor::timersAllocated = false; Motor * Motor::list[MAX_POSSIBLE_MOTORS] = { NULL, }; static TaskHandle_t complexHandlerTask; - -float TICKS_PER_DEGREE = 1.0/TICKS_TO_DEGREES; +const float TICKS_TO_DEGREES = (QUADRATUE_MULTIPLYER/(ENCODER_CPR*GEAR_BOX_RATIO/360.0)); +const float TICKS_PER_DEGREE = 1.0/TICKS_TO_DEGREES; const uint32_t LOOP_PERIOD_MS = 10; @@ -164,7 +168,6 @@ void Motor::MoveTo(float newTargetInDegrees, float speedDegPerSec) return; } - float Motor::StartMoveTo(float newTargetInDegrees, float speedDegPerSec) { currTrajectory.startPos = getCurrentDegrees() * TICKS_PER_DEGREE; @@ -294,9 +297,10 @@ void Motor::loop() if (closedLoopControl) { + //the logic here is sound, but could be streamlined if (mode == VELOCITY_MODE) { - if(fabs(currentEffort)<0.95)// stall detection -- could be less, could be better + if(fabs(currentEffort) < 0.95)// stall detection -- could be less, could be smarter Setpoint += deltaTargetTicks; } @@ -305,16 +309,17 @@ void Motor::loop() if(currTrajectory.FractionComplete() >= 1.0) { Setpoint = currTrajectory.targetPos; - } - else if(fabs(currentEffort)<0.5)// stall detection -- could be less, could be better + mode = MOTOR_IDLE; + } + else if(fabs(currentEffort) < 0.5)// stall detection -- could be less, could be better Setpoint += deltaTargetTicks; } float controlErr = Setpoint - nowEncoder; - if(fabs(currentEffort)<0.5) runningITerm += controlErr; + if(fabs(currentEffort) < 0.5) errorSum += controlErr; - currentEffort = controlErr * kP + runningITerm * kI; + currentEffort = controlErr * kP + errorSum * kI; } //this needs redoing @@ -336,7 +341,7 @@ void Motor::setGains(float p, float i, float d) { kP = p; kI = i; kD = d; - runningITerm = 0; + errorSum = 0; //portEXIT_CRITICAL(&mmux); } diff --git a/src/Motor.h b/src/Motor.h index ef81d0dd..a4c13d5c 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -10,15 +10,12 @@ #include #include #include + #define MAX_POSSIBLE_MOTORS 4 -#define ENCODER_CPR 12.0f -#define GEAR_BOX_RATIO 120.0f -#define QUADRATUE_MULTIPLYER 1.0f -#define TICKS_TO_DEGREES (QUADRATUE_MULTIPLYER/(ENCODER_CPR*GEAR_BOX_RATIO/360.0)) -#define I_TERM_SIZE 120.0f +//#define I_TERM_SIZE 120.0f enum interpolateMode { - LINEAR_INTERPOLATION, SINUSOIDAL_INTERPOLATION, VELOCITY_MODE, BEZIER, TRAPEZOIDAL + MOTOR_IDLE, LINEAR_INTERPOLATION, SINUSOIDAL_INTERPOLATION, VELOCITY_MODE, BEZIER, TRAPEZOIDAL }; struct Trajectory @@ -90,7 +87,7 @@ class Motor { /** * PID controller integral constant */ - float kI = 0.00004; + float kI = 0.0002; /** * PID controller derivitive constant */ @@ -98,7 +95,7 @@ class Motor { /** * a variable to store the running avarage for the integral term */ - float runningITerm = 0; + float errorSum = 0; /* * effort of the motor * @param a value from -1 to 1 representing effort From d9c147f9b075c4e0a0a5e7c180f8ec9edfb26361 Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 15 Aug 2020 08:05:08 -0400 Subject: [PATCH 07/17] Spelling, better variable names --- src/Motor.cpp | 20 ++++++++++---------- src/Motor.h | 48 ++++++++++++++++++++++++------------------------ 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 0ee0249b..5c3877aa 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -9,13 +9,13 @@ #define ENCODER_CPR 12.0f #define GEAR_BOX_RATIO 120.0f -#define QUADRATUE_MULTIPLYER 1.0f +#define QUADRATURE_MULTIPLYER 1.0f bool Motor::timersAllocated = false; Motor * Motor::list[MAX_POSSIBLE_MOTORS] = { NULL, }; static TaskHandle_t complexHandlerTask; -const float TICKS_TO_DEGREES = (QUADRATUE_MULTIPLYER/(ENCODER_CPR*GEAR_BOX_RATIO/360.0)); +const float TICKS_TO_DEGREES = (QUADRATURE_MULTIPLYER/(ENCODER_CPR*GEAR_BOX_RATIO/360.0)); const float TICKS_PER_DEGREE = 1.0/TICKS_TO_DEGREES; const uint32_t LOOP_PERIOD_MS = 10; @@ -100,7 +100,7 @@ void onMotorTimer(void *param) { } // } - Serial.println("ERROR Pid thread died!"); + Serial.println("ERROR: PID thread died!"); } /** @@ -132,7 +132,7 @@ Motor::~Motor() { // * SetSetpoint in degrees with time // * Set the setpoint for the motor in degrees // * @param newTargetInDegrees the new setpoint for the closed loop controller -// * @param msTimeDuration the number of miliseconds to get from current position to the new setpoint +// * @param msTimeDuration the number of milliseconds to get from current position to the new setpoint // */ // void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, // interpolateMode mode) { @@ -270,19 +270,19 @@ float Motor::SetDelta(float speedDegPerSec) * SetSpeed in degrees with time * Set the setpoint for the motor in degrees * @param newDegreesPerSecond the new speed in degrees per second - * @param miliseconds the number of miliseconds to run for - * @note a value of 0 miliseconds will set the motor into open-ended run mode + * @param durationMS the number of milliseconds to run for + * @note a value of 0 milliseconds will set the motor into open-ended run mode */ -void Motor::SetSpeed(float newDegreesPerSecond, long miliseconds) { - if (miliseconds < 1) { +void Motor::SetSpeed(float newDegreesPerSecond, long durationMS) { + if (durationMS < 1) { // 0 time will set up "Red Queen" (sic) interpolation SetSpeed(newDegreesPerSecond); return; } float currentPos = getCurrentDegrees(); float distance = currentPos - + (-newDegreesPerSecond * (((float) miliseconds) / 1000.0)); - SetSetpointWithTime(distance, miliseconds, LINEAR_INTERPOLATION); + + (-newDegreesPerSecond * (((float) durationMS) / 1000.0)); + SetSetpointWithTime(distance, durationMS, LINEAR_INTERPOLATION); } /** diff --git a/src/Motor.h b/src/Motor.h index a4c13d5c..83b3e48c 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -115,11 +115,11 @@ class Motor { */ float currentEffort = 0; /** - * PID controller Interpolation duration in miliseconds + * PID controller Interpolation duration in milliseconds */ // float duration = 0; /** - * PID controller Interpolation time in miliseconds that the interplation began + * PID controller Interpolation time in milliseconds that the interplation began */ // float startTime = 0; /** @@ -141,9 +141,9 @@ class Motor { /** * when using Red Queen mode for velocity interpolation, this is the amount of setpoint to add to the current setpoint - * every milisecond to maintain a smooth velocity trajectory. + * every LOOP_PERIOD_MS to maintain a smooth velocity trajectory. */ -// float milisecondPosIncrementForVelocity; +// float millisecondPosIncrementForVelocity; /** * \brief BEZIER Control Point 0 * @@ -186,7 +186,7 @@ class Motor { /** * This is a list of all of the Motor objects that have been attached. As a motor is attahed, * it adds itself to this list of Motor pointers. This list is read by the PID thread and each - * object in the list has loop() called. once every milisecond. + * object in the list has loop() called. once every LOOP_PERIOD_MS ms. */ static Motor * list[MAX_POSSIBLE_MOTORS]; @@ -285,10 +285,10 @@ class Motor { * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param miliseconds the number of miliseconds to get from current position to the new setpoint + * @param durationMS the number of milliseconds to get from current position to the new setpoint * param mode the interpolation mode */ - void SetSetpointWithTime(float newTargetInDegrees, long miliseconds, + void SetSetpointWithTime(float newTargetInDegrees, long durationMS, interpolateMode mode); /** * SetSpeed in degrees with time @@ -315,9 +315,9 @@ class Motor { * SetSpeed in degrees with time * Set the setpoint for the motor in degrees * @param newDegreesPerSecond the new speed in degrees per second - * @param miliseconds the number of miliseconds to run for + * @param durationMS the number of milliseconds to run for */ - void SetSpeed(float newDegreesPerSecond, long miliseconds); + void SetSpeed(float newDegreesPerSecond, long durationMS); /** * MoveTo in degrees with speed * Set the setpoint for the motor in degrees and the speed you want to get there @@ -369,12 +369,12 @@ class Motor { * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param miliseconds the number of miliseconds to get from current position to the new setpoint + * @param durationMS the number of milliseconds to get from current position to the new setpoint * use linear interoplation // */ // void SetSetpointWithLinearInterpolation(float newTargetInDegrees, - // long miliseconds) { - // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // long durationMS) { + // SetSetpointWithTime(newTargetInDegrees, durationMS, // LINEAR_INTERPOLATION); // } @@ -382,48 +382,48 @@ class Motor { * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param miliseconds the number of miliseconds to get from current position to the new setpoint + * @param durationMS the number of milliseconds to get from current position to the new setpoint * use sinusoidal interpolation // */ // void SetSetpointWithSinusoidalInterpolation(float newTargetInDegrees, - // long miliseconds) { - // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // long milliseconds) { + // SetSetpointWithTime(newTargetInDegrees, milliseconds, // SINUSOIDAL_INTERPOLATION); // } /** * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param miliseconds the number of miliseconds to get from current position to the new setpoint + * @param durationMS the number of milliseconds to get from current position to the new setpoint * @param Control_0 On a scale of 0 to 1, where should the first control point in the equation go * @param Control_1 On a scale of 0 to 1, where should the second control point in the equation go * use Bezier interpolation // */ // void SetSetpointWithBezierInterpolation(float newTargetInDegrees, - // long miliseconds, float Control_0=0.5, float Control_1=1.0) { + // long durationMS, float Control_0=0.5, float Control_1=1.0) { // BEZIER_P0=Control_0; // BEZIER_P1=Control_1; - // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // SetSetpointWithTime(newTargetInDegrees, durationMS, // BEZIER); // } /** * SetSetpoint in degrees with time * Set the setpoint for the motor in degrees * @param newTargetInDegrees the new setpoint for the closed loop controller - * @param miliseconds the number of miliseconds to get from current position to the new setpoint - * @param trapazoidalTime miliseconds for the ramping to take at the beginning and end. + * @param durationMS the number of milliseconds to get from current position to the new setpoint + * @param trapazoidalTime milliseconds for the ramping to take at the beginning and end. * * * use sinusoidal interpolation // */ // void SetSetpointWithTrapezoidalInterpolation(float newTargetInDegrees, - // long miliseconds, float trapazoidalTime) { - // if(trapazoidalTime*2>miliseconds){ - // SetSetpointWithSinusoidalInterpolation(newTargetInDegrees,miliseconds); + // long durationMS, float trapazoidalTime) { + // if(trapazoidalTime*2>durationMS){ + // SetSetpointWithSinusoidalInterpolation(newTargetInDegrees,durationMS); // return; // } // TRAPEZOIDAL_time=trapazoidalTime; - // SetSetpointWithTime(newTargetInDegrees, miliseconds, + // SetSetpointWithTime(newTargetInDegrees, durationMS, // TRAPEZOIDAL); // } /** From 4ddb46a63bd8c2a8406599830a42341da4458315 Mon Sep 17 00:00:00 2001 From: Greg Date: Mon, 17 Aug 2020 09:34:38 -0400 Subject: [PATCH 08/17] Smart aleck Author edit --- src/Motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 5c3877aa..eac8b91b 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -2,7 +2,7 @@ * Motor.cpp * * Created on: May 31, 2020 - * Author: hephaestus + * Author: hephaestus f. gcl8a */ #include From 36d7ff1b127f725717e2d69f7b682a89d0959166 Mon Sep 17 00:00:00 2001 From: Greg Date: Thu, 20 Aug 2020 19:43:56 -0400 Subject: [PATCH 09/17] Proper MotorTest example --- examples/MotorTest/MotorTest.ino | 113 +++++++++++++++++++++++-------- 1 file changed, 83 insertions(+), 30 deletions(-) diff --git a/examples/MotorTest/MotorTest.ino b/examples/MotorTest/MotorTest.ino index 4d8f1c52..a100126f 100644 --- a/examples/MotorTest/MotorTest.ino +++ b/examples/MotorTest/MotorTest.ino @@ -1,20 +1,38 @@ +/* + * This program will wait for the button to be pressed and then: + * command the left motor to spin at 60 rpm, + * wait 5 seconds, + * and then stop the motor. + * While the motor is spinning, the program will print out how much it has turned (in degrees). + */ + #include +#include +#include +#include #include Motor motor1; Motor motor2; -bool upDown=false; +// pin definitions https://wpiroboticsengineering.github.io/RBE1001Lib/RBE1001Lib_8h.html#define-members +const int buttonPin = BOOT_FLAG_PIN ; + /* * This is the standard setup function that is called when the ESP32 is rebooted * It is used to initialize anything that needs to be done once. */ -void setup() { +void setup() +{ + //delay(2000); + // This will initialize the Serial as 115200 for prints Serial.begin(115200); Motor::allocateTimer(0); // pin definitions https://wpiroboticsengineering.github.io/RBE1001Lib/RBE1001Lib_8h.html#define-members motor1.attach(MOTOR1_PWM, MOTOR1_DIR, MOTOR1_ENCA, MOTOR1_ENCB); motor2.attach(MOTOR2_PWM, MOTOR2_DIR, MOTOR2_ENCA, MOTOR2_ENCB); + //explicitly make the button pin an input and engage the internal pullup resistor + pinMode(buttonPin, INPUT_PULLUP); } @@ -22,32 +40,67 @@ void setup() { * The main loop for the program. The loop function is repeatedly called * once the ESP32 is started. */ -void loop() { - upDown=!upDown; - motor1.SetSetpointWithLinearInterpolation(upDown?3600:0, 8000); - //motor2.SetSetpointWithLinearInterpolation(upDown?360:0, 2000); - //motor2.SetSetpointWithBezierInterpolation(upDown?3600:0, 8000,0.45,1); - motor2.SetSetpointWithTrapezoidalInterpolation(upDown?3600:0, 8000, 500); - double peak1 = 0; - double peak2 =0; - - for(int i=0;i<400;i++){ - if(abs(motor1.getDegreesPerSecond())>peak1){ - peak1=abs(motor1.getDegreesPerSecond()); - } - if(abs(motor2.getDegreesPerSecond())>peak2){ - peak2=abs(motor2.getDegreesPerSecond()); - } - delay(20); - Serial.println("motor compared "+String(motor2.getInterpolationUnitIncrement()-motor1.getInterpolationUnitIncrement())+ - +" Interp "+String(motor2.getInterpolationUnitIncrement())+ - +" Vel 1 "+String(motor1.getDegreesPerSecond())+" Vel 2 "+String(motor2.getDegreesPerSecond())); - } - delay(100); - Serial.println("Count 1 "+String(motor1.getCurrentDegrees())+ - " Count 2 "+String(motor2.getCurrentDegrees())); - delay(5000); - - - } +void loop() +{ + //The following line will cause the program to wait indefinitely until the button is pressed + while(digitalRead(buttonPin)) {} + + Serial.println("Setting speeds"); + + motor1.SetSpeed(180); + motor2.SetSpeed(-180); + + uint32_t startTime = millis(); //note when the motor started + + + while(millis() - startTime < 3000) //run for 3 seconds + { + Serial.print(motor1.getCurrentDegrees()); //motor1 position + Serial.print('\t'); //TAB character + Serial.print(motor2.getCurrentDegrees()); //motor2 position + Serial.print('\n'); //newline character + } + + // stop the motor + motor1.SetSpeed(0); + motor2.SetSpeed(0); + + //The following line will cause the program to wait indefinitely until the button is pressed + while(digitalRead(buttonPin)) {} + + //motor1.SetEffort(-0.1); + motor2.SetEffort(0.3); + + startTime = millis(); //note when the motor started + + while(millis() - startTime < 3000) //run for 3 seconds + { + Serial.print(motor1.getCurrentDegrees()); //motor1 position + Serial.print('\t'); //TAB character + Serial.print(motor2.getCurrentDegrees()); //motor2 position + Serial.print('\n'); //newline character + } + + // stop the motor + motor1.SetEffort(0); + motor2.SetEffort(0); + + //The following line will cause the program to wait indefinitely until the button is pressed + while(digitalRead(buttonPin)) {} + motor1.StartMoveFor(720, -540); + motor2.MoveFor(-720, 540); + + //The following line will cause the program to wait indefinitely until the button is pressed + while(digitalRead(buttonPin)) {} + + motor1.StartMoveTo(0, -180); + motor2.MoveTo(0, 180); + while(digitalRead(buttonPin)) //run for 3 seconds + { + Serial.print(motor1.getCurrentDegrees()); //motor1 position + Serial.print('\t'); //TAB character + Serial.print(motor2.getCurrentDegrees()); //motor2 position + Serial.print('\n'); //newline character + } +} \ No newline at end of file From 11e9f2c3ceafccc9caacd29f4a6d66db7689a5c8 Mon Sep 17 00:00:00 2001 From: Greg Date: Thu, 20 Aug 2020 19:55:00 -0400 Subject: [PATCH 10/17] Example update --- examples/MotorTest/MotorTest.ino | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/MotorTest/MotorTest.ino b/examples/MotorTest/MotorTest.ino index a100126f..1a905956 100644 --- a/examples/MotorTest/MotorTest.ino +++ b/examples/MotorTest/MotorTest.ino @@ -23,8 +23,6 @@ const int buttonPin = BOOT_FLAG_PIN ; */ void setup() { - //delay(2000); - // This will initialize the Serial as 115200 for prints Serial.begin(115200); Motor::allocateTimer(0); @@ -68,7 +66,7 @@ void loop() //The following line will cause the program to wait indefinitely until the button is pressed while(digitalRead(buttonPin)) {} - //motor1.SetEffort(-0.1); + motor1.SetEffort(-0.1); motor2.SetEffort(0.3); startTime = millis(); //note when the motor started From ade77a8ce41d06093b491a63698b88b2387ca1b3 Mon Sep 17 00:00:00 2001 From: Greg Date: Thu, 20 Aug 2020 22:12:05 -0400 Subject: [PATCH 11/17] Adjusted some coefficients. --- examples/MotorTest/MotorTest.ino | 4 ++-- src/Motor.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/MotorTest/MotorTest.ino b/examples/MotorTest/MotorTest.ino index 1a905956..774457ce 100644 --- a/examples/MotorTest/MotorTest.ino +++ b/examples/MotorTest/MotorTest.ino @@ -66,8 +66,8 @@ void loop() //The following line will cause the program to wait indefinitely until the button is pressed while(digitalRead(buttonPin)) {} - motor1.SetEffort(-0.1); - motor2.SetEffort(0.3); + motor1.SetEffort(0.35); + motor2.SetEffort(-0.25); startTime = millis(); //note when the motor started diff --git a/src/Motor.cpp b/src/Motor.cpp index eac8b91b..45cb69ad 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -300,7 +300,7 @@ void Motor::loop() //the logic here is sound, but could be streamlined if (mode == VELOCITY_MODE) { - if(fabs(currentEffort) < 0.95)// stall detection -- could be less, could be smarter + if(fabs(currentEffort) < 0.75)// stall detection -- could be better Setpoint += deltaTargetTicks; } @@ -311,13 +311,13 @@ void Motor::loop() Setpoint = currTrajectory.targetPos; mode = MOTOR_IDLE; } - else if(fabs(currentEffort) < 0.5)// stall detection -- could be less, could be better + else if(fabs(currentEffort) < 0.75) // stall detection -- could be better Setpoint += deltaTargetTicks; } float controlErr = Setpoint - nowEncoder; - if(fabs(currentEffort) < 0.5) errorSum += controlErr; + if(fabs(currentEffort) < 0.5) errorSum += controlErr; //limit windup currentEffort = controlErr * kP + errorSum * kI; } @@ -330,6 +330,7 @@ void Motor::loop() cachedSpeed = err / (0.05); // ticks per second prevousCount = nowEncoder; } + // invert the effort so that the set speed and set effort match SetEffortLocal(currentEffort); } From 9ed32c7a3c2fa2cc1f52eb26f87e65e3c524466d Mon Sep 17 00:00:00 2001 From: Greg Date: Sat, 22 Aug 2020 08:54:06 -0400 Subject: [PATCH 12/17] nop --- examples/RCCTL/RCCTL.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/RCCTL/RCCTL.ino b/examples/RCCTL/RCCTL.ino index 33cf7288..f9f927be 100644 --- a/examples/RCCTL/RCCTL.ino +++ b/examples/RCCTL/RCCTL.ino @@ -24,7 +24,6 @@ WebPage buttonPage; WifiManager manager; - Timer dashboardUpdateTimer; // times when the dashboard should update /* * This is the standard setup function that is called when the ESP32 is rebooted From f15ff364327bbc31ad63b2ca3fb13d166213bb2a Mon Sep 17 00:00:00 2001 From: Greg Date: Mon, 24 Aug 2020 11:47:23 -0400 Subject: [PATCH 13/17] added rbe1001lib include --- examples/week01/button/button.ino | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/week01/button/button.ino b/examples/week01/button/button.ino index 0aeff312..523ec6a5 100644 --- a/examples/week01/button/button.ino +++ b/examples/week01/button/button.ino @@ -1,4 +1,6 @@ - // pin definitions https://wpiroboticsengineering.github.io/RBE1001Lib/RBE1001Lib_8h.html#define-members +#include + +// pin definitions https://wpiroboticsengineering.github.io/RBE1001Lib/RBE1001Lib_8h.html#define-members const int buttonPin = BOOT_FLAG_PIN; // the number of the pushbutton pin const int ledPin = 13; // the number of the LED pin From 9b8c42153892f53ec3549881a87d8a4b24422d59 Mon Sep 17 00:00:00 2001 From: Greg Date: Tue, 25 Aug 2020 18:06:14 -0400 Subject: [PATCH 14/17] Cut default gains. --- src/Motor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Motor.h b/src/Motor.h index 83b3e48c..f20f7f9b 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -83,11 +83,11 @@ class Motor { /** * PID controller proportional constant */ - float kP = 0.01; + float kP = 0.005; /** * PID controller integral constant */ - float kI = 0.0002; + float kI = 0.00005; /** * PID controller derivitive constant */ From 9874e95991c0eabdb97217ddf8b3b02148f82e0e Mon Sep 17 00:00:00 2001 From: Greg Date: Tue, 25 Aug 2020 18:28:33 -0400 Subject: [PATCH 15/17] Update Motor.cpp --- src/Motor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 45cb69ad..1bcbaac3 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -256,9 +256,9 @@ void Motor::SetSpeed(float speedDegPerSec) float Motor::SetDelta(float speedDegPerSec) { deltaTargetTicks = speedDegPerSec * TICKS_PER_DEGREE * LOOP_PERIOD_MS / 1000.0; - Serial.println("Setting Speed "+String(speedDegPerSec)+ - " increment "+String(deltaTargetTicks) - ); + //Serial.println("Setting Speed "+String(speedDegPerSec)+ + // " increment "+String(deltaTargetTicks) + //); Setpoint = nowEncoder; closedLoopControl = true; From 8cceb588301d07ea0cec8f8e9a60a239fa3c5b90 Mon Sep 17 00:00:00 2001 From: Greg Date: Fri, 4 Sep 2020 16:28:19 -0400 Subject: [PATCH 16/17] Messing with motor.cpp --- examples/MotorSpeedTest/MotorSpeedTest.ino | 44 ++++++++++++++++++++++ src/RBE1001Lib.h | 28 ++++++++------ 2 files changed, 61 insertions(+), 11 deletions(-) create mode 100644 examples/MotorSpeedTest/MotorSpeedTest.ino diff --git a/examples/MotorSpeedTest/MotorSpeedTest.ino b/examples/MotorSpeedTest/MotorSpeedTest.ino new file mode 100644 index 00000000..ca9cf194 --- /dev/null +++ b/examples/MotorSpeedTest/MotorSpeedTest.ino @@ -0,0 +1,44 @@ +#include +#include + +Motor left_motor; +Motor right_motor; +bool upDown=false; +/* + * This is the standard setup function that is called when the ESP32 is rebooted + * It is used to initialize anything that needs to be done once. + */ +void setup() { + // This will initialize the Serial as 115200 for prints + Serial.begin(115200); + Motor::allocateTimer(0); + // pin definitions https://wpiroboticsengineering.github.io/RBE1001Lib/RBE1001Lib_8h.html#define-members + left_motor.attach(MOTOR_LEFT_PWM, MOTOR_LEFT_DIR, MOTOR_LEFT_ENCA, MOTOR_LEFT_ENCB); + right_motor.attach(MOTOR_RIGHT_PWM, MOTOR_RIGHT_DIR, MOTOR_RIGHT_ENCA, MOTOR_RIGHT_ENCB); +} + + +/* + * The main loop for the program. The loop function is repeatedly called + * once the ESP32 is started. + */ +void loop() +{ + float speed = 360 * sin(2*3.14*(millis()%10000)/10000.); + left_motor.setSpeed(speed); + right_motor.setSpeed(speed); + + Serial.print("SP: "); + Serial.print(speed); + Serial.print('\t'); + + Serial.print("L: "); + Serial.print(left_motor.getDegreesPerSecond()); + Serial.print('\t'); + + Serial.print("R: "); + Serial.print(right_motor.getDegreesPerSecond()); + Serial.print('\n'); + + delay(100); +} diff --git a/src/RBE1001Lib.h b/src/RBE1001Lib.h index b2767cb7..ae422eb2 100644 --- a/src/RBE1001Lib.h +++ b/src/RBE1001Lib.h @@ -1,4 +1,10 @@ - #pragma once +#pragma once + +#include +#include +#include +#include + #include "Rangefinder.h" #include "Motor.h" @@ -15,8 +21,8 @@ #define SPI_SS 5 // Ultrasonics -#define SIDE_ULTRASONIC_TRIG 17 -#define SIDE_ULTRASONIC_ECHO 16 +#define SIDE_ULTRASONIC_TRIG 23 +#define SIDE_ULTRASONIC_ECHO 22 //A4 #define LEFT_LINE_SENSE 36 @@ -34,29 +40,29 @@ /** * Drive motor 1 10Khz full duty PWM pin */ -#define MOTOR1_PWM 13 +#define MOTOR_LEFT_PWM 13 /** * Pin for setting the direction of the H-Bridge */ //A5 -#define MOTOR1_DIR 4 +#define MOTOR_LEFT_DIR 4 /** * Drive motor 2 10Khz full duty PWM pin */ -#define MOTOR2_PWM 12 +#define MOTOR_RIGHT_PWM 12 /** * Pin for setting the direction of the H-Bridge */ //A1 -#define MOTOR2_DIR 25 +#define MOTOR_RIGHT_DIR 25 //Encoder pins -#define MOTOR1_ENCA 27 +#define MOTOR_LEFT_ENCA 27 //A0 -#define MOTOR1_ENCB 26 +#define MOTOR_LEFT_ENCB 26 -#define MOTOR2_ENCA 32 -#define MOTOR2_ENCB 14 +#define MOTOR_RIGHT_ENCA 32 +#define MOTOR_RIGHT_ENCB 14 From a518524ff596daa22b8fc4da0c5617ed8a4dfc22 Mon Sep 17 00:00:00 2001 From: Greg Date: Fri, 4 Sep 2020 16:28:28 -0400 Subject: [PATCH 17/17] motor tests --- src/Motor.cpp | 4 ++-- src/Motor.h | 2 +- src/RBE1001Lib.h | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Motor.cpp b/src/Motor.cpp index 1bcbaac3..772ab25d 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -247,7 +247,7 @@ void Motor::MoveFor(float deltaTargetInDegrees, float speedDegPerSec) * * @param newDegreesPerSecond the new speed in degrees per second */ -void Motor::SetSpeed(float speedDegPerSec) +void Motor::setSpeed(float speedDegPerSec) { SetDelta(speedDegPerSec); mode = VELOCITY_MODE; @@ -276,7 +276,7 @@ float Motor::SetDelta(float speedDegPerSec) void Motor::SetSpeed(float newDegreesPerSecond, long durationMS) { if (durationMS < 1) { // 0 time will set up "Red Queen" (sic) interpolation - SetSpeed(newDegreesPerSecond); + setSpeed(newDegreesPerSecond); return; } float currentPos = getCurrentDegrees(); diff --git a/src/Motor.h b/src/Motor.h index f20f7f9b..8e9313b2 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -307,7 +307,7 @@ class Motor { * * @param newDegreesPerSecond the new speed in degrees per second */ - void SetSpeed(float newDegreesPerSecond); + void setSpeed(float newDegreesPerSecond); float SetDelta(float newDegreesPerSecond); diff --git a/src/RBE1001Lib.h b/src/RBE1001Lib.h index ae422eb2..d6db9104 100644 --- a/src/RBE1001Lib.h +++ b/src/RBE1001Lib.h @@ -57,9 +57,9 @@ #define MOTOR_RIGHT_DIR 25 //Encoder pins -#define MOTOR_LEFT_ENCA 27 +#define MOTOR_LEFT_ENCA 26 //A0 -#define MOTOR_LEFT_ENCB 26 +#define MOTOR_LEFT_ENCB 27 #define MOTOR_RIGHT_ENCA 32