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/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/examples/MotorTest/MotorTest.ino b/examples/MotorTest/MotorTest.ino index 4d8f1c52..774457ce 100644 --- a/examples/MotorTest/MotorTest.ino +++ b/examples/MotorTest/MotorTest.ino @@ -1,20 +1,36 @@ +/* + * 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() +{ // 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 +38,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.35); + motor2.SetEffort(-0.25); + + 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 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 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 diff --git a/src/Motor.cpp b/src/Motor.cpp index 4b23df64..772ab25d 100644 --- a/src/Motor.cpp +++ b/src/Motor.cpp @@ -2,15 +2,23 @@ * Motor.cpp * * Created on: May 31, 2020 - * Author: hephaestus + * Author: hephaestus f. gcl8a */ #include +#define ENCODER_CPR 12.0f +#define GEAR_BOX_RATIO 120.0f +#define QUADRATURE_MULTIPLYER 1.0f + bool Motor::timersAllocated = false; Motor * Motor::list[MAX_POSSIBLE_MOTORS] = { NULL, }; static TaskHandle_t complexHandlerTask; -//portMUX_TYPE mmux = portMUX_INITIALIZER_UNLOCKED; + +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; float myFmapBounded(float x, float in_min, float in_max, float out_min, float out_max) { @@ -24,60 +32,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); @@ -91,7 +100,7 @@ void onMotorTimer(void *param) { } // } - Serial.println("ERROR Pid thread died!"); + Serial.println("ERROR: PID thread died!"); } /** @@ -119,30 +128,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 = Setpoint; - 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 milliseconds 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 +162,59 @@ void Motor::SetSetpointWithTime(float newTargetInDegrees, long msTimeDuration, */ 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());} + + return; } +float Motor::StartMoveTo(float newTargetInDegrees, float speedDegPerSec) +{ + currTrajectory.startPos = getCurrentDegrees() * TICKS_PER_DEGREE; + currTrajectory.targetPos = newTargetInDegrees * TICKS_PER_DEGREE; + + if(currTrajectory.targetPos > currTrajectory.startPos) speedDegPerSec = fabs(speedDegPerSec); + else speedDegPerSec = -fabs(speedDegPerSec); + + SetDelta(speedDegPerSec); + mode = LINEAR_INTERPOLATION; + + return newTargetInDegrees; +} float Motor::StartMoveFor(float deltaTargetInDegrees, float speedDegPerSec) { - float newSetPoint = getCurrentDegrees() + deltaTargetInDegrees; - SetSetpointWithTime(newSetPoint, - fabs(deltaTargetInDegrees / speedDegPerSec) * 1000.0, - SINUSOIDAL_INTERPOLATION); - return newSetPoint; + float targetPosDeg = getCurrentDegrees() + deltaTargetInDegrees; + return StartMoveTo(targetPosDeg, speedDegPerSec); } -/** - * \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); - } -} +// /** +// * \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 +227,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,41 +247,42 @@ 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; } /** * 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); + 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); } /** @@ -266,40 +290,39 @@ 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(abs(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) + { + //the logic here is sound, but could be streamlined + if (mode == VELOCITY_MODE) + { + if(fabs(currentEffort) < 0.75)// stall detection -- could be better + Setpoint += deltaTargetTicks; + } + + else + { + if(currTrajectory.FractionComplete() >= 1.0) + { + Setpoint = currTrajectory.targetPos; + mode = MOTOR_IDLE; } + else if(fabs(currentEffort) < 0.75) // stall detection -- 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 - runntingITerm += controlErr; - if(getInterpolationUnitIncrement()<1){ - // no i term during interpolation - runntingITerm=0; - } - currentEffort = controlErr * kP + ((runntingITerm / I_TERM_SIZE) * kI); + if(fabs(currentEffort) < 0.5) errorSum += controlErr; //limit windup - //portEXIT_CRITICAL(&mmux); + currentEffort = controlErr * kP + errorSum * kI; } + //this needs redoing interruptCountForVelocity++; if (interruptCountForVelocity == 50) { interruptCountForVelocity = 0; @@ -307,9 +330,9 @@ 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); - } /** * PID gains for the PID controller @@ -319,7 +342,7 @@ void Motor::setGains(float p, float i, float d) { kP = p; kI = i; kD = d; - runntingITerm = 0; + errorSum = 0; //portEXIT_CRITICAL(&mmux); } @@ -361,16 +384,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; - //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 a5ba8492..8e9313b2 100644 --- a/src/Motor.h +++ b/src/Motor.h @@ -10,15 +10,28 @@ #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 +{ + //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 +83,11 @@ class Motor { /** * PID controller proportional constant */ - float kP = 0.05; + float kP = 0.005; /** * PID controller integral constant */ - float kI = 0.06; + float kI = 0.00005; /** * PID controller derivitive constant */ @@ -82,7 +95,7 @@ class Motor { /** * a variable to store the running avarage for the integral term */ - float runntingITerm = 0; + float errorSum = 0; /* * effort of the motor * @param a value from -1 to 1 representing effort @@ -102,25 +115,25 @@ class Motor { */ float currentEffort = 0; /** - * PID controller Interpolation duration in miliseconds + * PID controller Interpolation duration in milliseconds */ - float duration = 0; +// 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; +// 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 */ @@ -128,26 +141,33 @@ 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 * * 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: /** @@ -166,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]; @@ -265,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 @@ -287,14 +307,17 @@ class Motor { * * @param newDegreesPerSecond the new speed in degrees per second */ - void SetSpeed(float newDegreesPerSecond); + void setSpeed(float newDegreesPerSecond); + + float SetDelta(float newDegreesPerSecond); + /** * 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 @@ -303,6 +326,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 @@ -334,72 +360,72 @@ 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 * 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, - LINEAR_INTERPOLATION); - } + // */ + // void SetSetpointWithLinearInterpolation(float newTargetInDegrees, + // long durationMS) { + // SetSetpointWithTime(newTargetInDegrees, durationMS, + // LINEAR_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 * use sinusoidal interpolation - */ - void SetSetpointWithSinusoidalInterpolation(float newTargetInDegrees, - long miliseconds) { - SetSetpointWithTime(newTargetInDegrees, miliseconds, - SINUSOIDAL_INTERPOLATION); - } + // */ + // void SetSetpointWithSinusoidalInterpolation(float newTargetInDegrees, + // 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) { - BEZIER_P0=Control_0; - BEZIER_P1=Control_1; - SetSetpointWithTime(newTargetInDegrees, miliseconds, - BEZIER); - } + // */ + // void SetSetpointWithBezierInterpolation(float newTargetInDegrees, + // long durationMS, float Control_0=0.5, float Control_1=1.0) { + // BEZIER_P0=Control_0; + // BEZIER_P1=Control_1; + // 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); - return; - } - TRAPEZOIDAL_time=trapazoidalTime; - SetSetpointWithTime(newTargetInDegrees, miliseconds, - TRAPEZOIDAL); - } + // */ + // void SetSetpointWithTrapezoidalInterpolation(float newTargetInDegrees, + // long durationMS, float trapazoidalTime) { + // if(trapazoidalTime*2>durationMS){ + // SetSetpointWithSinusoidalInterpolation(newTargetInDegrees,durationMS); + // return; + // } + // TRAPEZOIDAL_time=trapazoidalTime; + // SetSetpointWithTime(newTargetInDegrees, durationMS, + // TRAPEZOIDAL); + // } /** * PID gains for the PID controller */ diff --git a/src/RBE1001Lib.h b/src/RBE1001Lib.h index b2767cb7..d6db9104 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 26 //A0 -#define MOTOR1_ENCB 26 +#define MOTOR_LEFT_ENCB 27 -#define MOTOR2_ENCA 32 -#define MOTOR2_ENCB 14 +#define MOTOR_RIGHT_ENCA 32 +#define MOTOR_RIGHT_ENCB 14