Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed QEI biquadFilter
Diff: controller.cpp
- Revision:
- 7:a80cb6b06320
- Parent:
- 2:fc869e45e672
- Child:
- 12:8295c02d740f
--- a/controller.cpp Wed Nov 02 09:26:38 2016 +0000 +++ b/controller.cpp Wed Nov 02 16:29:13 2016 +0000 @@ -1,29 +1,110 @@ #include "controller.h" -bool constantMovementTo(Arm arm, float length) { - const float velocity = 2; - float initialLength = arm.getLength(); - float dLength = length-initialLength; +/* + Constructor +*/ + +RobotController::RobotController(): + upperArmController(0.2, 0, 0), + lowerArmController(0.3, 0, 0), + debug(USBTX, USBRX), + robot() { + + upperArmLengthReference = 0; + lowerArmLengthReference = 0; + + MOVE_BY_REFERENCE = false; + PAINT_MOVE_UP = false; + PAINT_MOVE_DOWN = false; + + ticker.attach(this, &RobotController::doTick, tick); + +} + +/* + Private Methods +*/ + +void RobotController::doTick() { + robot.update(); + + if (MOVE_BY_REFERENCE) { + float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick); + float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick); + + robot.setUpperArmVelocity(upperOutput); + robot.setLowerArmVelocity(lowerOutput); + } - if (dLength < 0) { - arm.setVelocity(-1 * velocity); - while (dLength < 0) { - // Wait - } - arm.setVelocity(0); - return true; - } else { - arm.setVelocity(velocity); - while (dLength > 0) { - // Wait - } - arm.setVelocity(0); - return true; + if (PAINT_MOVE_UP) { + + } + + if (PAINT_MOVE_DOWN) { + } } +/* + Public methods +*/ + + +// Set the reference lengths for the arms that correspond to this roller coordinate +void RobotController::moveTo(float x, float y) { + float upper, lower; + getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference + upperArmLengthReference = upper; + lowerArmLengthReference = lower; + + PAINT_MOVE_UP = false; + PAINT_MOVE_DOWN = false; + MOVE_BY_REFERENCE = true; +} + +// Set the arm lengths +void RobotController::setArmLengths(float upper, float lower) { + upperArmLengthReference = upper; + lowerArmLengthReference = lower; + + PAINT_MOVE_UP = false; + PAINT_MOVE_DOWN = false; + MOVE_BY_REFERENCE = true; + + debug.printf("Set arm length reference to: Upper %f, Lower %f\n\r", upper, lower); +} + + + + +/* + Robot queries +*/ + +float RobotController::getUpperArmLength() { + return robot.getUpperArmLength(); +} + +float RobotController::getLowerArmLength() { + return robot.getLowerArmLength(); +} + +bool RobotController::isKilled() { + return robot.isKilled(); +} + +// Get robot pointer +Robot* RobotController::getRobot() { + return &robot; +} + + + + + +