Project Paint / Mbed 2 deprecated arm_control

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

controller.cpp@12:8295c02d740f, 2016-11-03 (annotated)

Committer:
ronvbree
Date:
Thu Nov 03 13:08:44 2016 +0000
Revision:
12:8295c02d740f
Parent:
7:a80cb6b06320
Child:
14:551049a798a3
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ronvbree 0:494acf21d3bc 1 #include "controller.h"
ronvbree 0:494acf21d3bc 2
ronvbree 7:a80cb6b06320 3 /*
ronvbree 7:a80cb6b06320 4 Constructor
ronvbree 7:a80cb6b06320 5 */
ronvbree 7:a80cb6b06320 6
ronvbree 7:a80cb6b06320 7 RobotController::RobotController():
ronvbree 7:a80cb6b06320 8 upperArmController(0.2, 0, 0),
ronvbree 12:8295c02d740f 9 lowerArmController(1.0, 0, 0),
ronvbree 12:8295c02d740f 10 upperXController(6, 0, 0),
ronvbree 12:8295c02d740f 11 // lowerXController(1.9, 0, 0),
ronvbree 12:8295c02d740f 12 lowerXController(1, 0, 0.01),
ronvbree 7:a80cb6b06320 13 debug(USBTX, USBRX),
ronvbree 7:a80cb6b06320 14 robot() {
ronvbree 12:8295c02d740f 15
ronvbree 12:8295c02d740f 16 // Set initial arm length as reference value
ronvbree 12:8295c02d740f 17 upperArmLengthReference = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 18 lowerArmLengthReference = robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 19
ronvbree 12:8295c02d740f 20 // Set initial roller position as reference value
ronvbree 12:8295c02d740f 21 float x,y; // Can't pass volatiles by reference
ronvbree 12:8295c02d740f 22 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 23 referenceX = x;
ronvbree 12:8295c02d740f 24 referenceY = y;
ronvbree 12:8295c02d740f 25
ronvbree 12:8295c02d740f 26 // Set movement flags
ronvbree 7:a80cb6b06320 27 MOVE_BY_REFERENCE = false;
ronvbree 7:a80cb6b06320 28 PAINT_MOVE_UP = false;
ronvbree 7:a80cb6b06320 29 PAINT_MOVE_DOWN = false;
ronvbree 7:a80cb6b06320 30
ronvbree 12:8295c02d740f 31 // Start ticker
ronvbree 7:a80cb6b06320 32 ticker.attach(this, &RobotController::doTick, tick);
ronvbree 7:a80cb6b06320 33
ronvbree 7:a80cb6b06320 34 }
ronvbree 7:a80cb6b06320 35
ronvbree 7:a80cb6b06320 36 /*
ronvbree 7:a80cb6b06320 37 Private Methods
ronvbree 7:a80cb6b06320 38 */
ronvbree 7:a80cb6b06320 39
ronvbree 7:a80cb6b06320 40 void RobotController::doTick() {
ronvbree 7:a80cb6b06320 41 robot.update();
ronvbree 7:a80cb6b06320 42
ronvbree 7:a80cb6b06320 43 if (MOVE_BY_REFERENCE) {
ronvbree 7:a80cb6b06320 44 float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick);
ronvbree 7:a80cb6b06320 45 float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick);
ronvbree 7:a80cb6b06320 46
ronvbree 7:a80cb6b06320 47 robot.setUpperArmVelocity(upperOutput);
ronvbree 7:a80cb6b06320 48 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 49 }
ronvbree 0:494acf21d3bc 50
ronvbree 7:a80cb6b06320 51 if (PAINT_MOVE_UP) {
ronvbree 12:8295c02d740f 52 float x,y;
ronvbree 12:8295c02d740f 53 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 54
ronvbree 12:8295c02d740f 55 float xError = referenceX - x;
ronvbree 12:8295c02d740f 56 float yError = topY - y;
ronvbree 12:8295c02d740f 57
ronvbree 12:8295c02d740f 58 float upperOutput = upperXController.control(xError, tick);
ronvbree 12:8295c02d740f 59 float lowerOutput = lowerArmController.control(yError, tick);
ronvbree 12:8295c02d740f 60
ronvbree 12:8295c02d740f 61 robot.setUpperArmVelocity(-1 * upperOutput);
ronvbree 12:8295c02d740f 62 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 63
ronvbree 7:a80cb6b06320 64 }
ronvbree 7:a80cb6b06320 65
ronvbree 7:a80cb6b06320 66 if (PAINT_MOVE_DOWN) {
ronvbree 12:8295c02d740f 67 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 68 float x,y;
ronvbree 12:8295c02d740f 69 float upperArmLength = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 70 float lowerArmLength = robot.getLowerArmLength();
ronvbree 12:8295c02d740f 71 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 72
ronvbree 12:8295c02d740f 73 // Get controller output for leading arm (upper arm)
ronvbree 12:8295c02d740f 74 float yError = bottomY - y;
ronvbree 12:8295c02d740f 75 float upperOutput = -1 * upperArmController.control(yError, tick);
ronvbree 12:8295c02d740f 76
ronvbree 12:8295c02d740f 77 // Predict influence on x position
ronvbree 12:8295c02d740f 78 float futureUpperArmLength = robot.getUpperArmLength() + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
ronvbree 12:8295c02d740f 79 float futureX, futureY;
ronvbree 12:8295c02d740f 80 getRollerPositionForArmLengths(futureUpperArmLength, robot.getLowerArmLength(), futureX, futureY);
ronvbree 12:8295c02d740f 81
ronvbree 12:8295c02d740f 82 // Calculate lower arm length that is needed to compensate for the leading arm
ronvbree 12:8295c02d740f 83 float upper,lower;
ronvbree 12:8295c02d740f 84 getArmLengthsForRollerPosition(referenceX, futureY, upper, lower);
ronvbree 12:8295c02d740f 85
ronvbree 12:8295c02d740f 86 // Calculate compensating arm velocity based on error
ronvbree 12:8295c02d740f 87 float compensatingArmError = lower - lowerArmLength;
ronvbree 12:8295c02d740f 88 float lowerOutput = lowerXController.control(compensatingArmError, tick);
ronvbree 12:8295c02d740f 89
ronvbree 12:8295c02d740f 90
ronvbree 12:8295c02d740f 91 // float xError = referenceX - futureX;
ronvbree 12:8295c02d740f 92
ronvbree 12:8295c02d740f 93 // float lowerOutput = -1 * lowerXController.control(xError, tick);
ronvbree 12:8295c02d740f 94
ronvbree 12:8295c02d740f 95 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 96 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 97
ronvbree 0:494acf21d3bc 98 }
ronvbree 0:494acf21d3bc 99 }
ronvbree 0:494acf21d3bc 100
ronvbree 7:a80cb6b06320 101 /*
ronvbree 7:a80cb6b06320 102 Public methods
ronvbree 7:a80cb6b06320 103 */
ronvbree 7:a80cb6b06320 104
ronvbree 7:a80cb6b06320 105
ronvbree 7:a80cb6b06320 106 // Set the reference lengths for the arms that correspond to this roller coordinate
ronvbree 7:a80cb6b06320 107 void RobotController::moveTo(float x, float y) {
ronvbree 12:8295c02d740f 108 clearFlags();
ronvbree 7:a80cb6b06320 109 float upper, lower;
ronvbree 7:a80cb6b06320 110 getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference
ronvbree 7:a80cb6b06320 111 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 112 lowerArmLengthReference = lower;
ronvbree 12:8295c02d740f 113
ronvbree 7:a80cb6b06320 114 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 115 }
ronvbree 7:a80cb6b06320 116
ronvbree 7:a80cb6b06320 117 // Set the arm lengths
ronvbree 7:a80cb6b06320 118 void RobotController::setArmLengths(float upper, float lower) {
ronvbree 12:8295c02d740f 119 clearFlags();
ronvbree 12:8295c02d740f 120
ronvbree 7:a80cb6b06320 121 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 122 lowerArmLengthReference = lower;
ronvbree 7:a80cb6b06320 123
ronvbree 7:a80cb6b06320 124 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 125
ronvbree 7:a80cb6b06320 126 debug.printf("Set arm length reference to: Upper %f, Lower %f\n\r", upper, lower);
ronvbree 7:a80cb6b06320 127 }
ronvbree 7:a80cb6b06320 128
ronvbree 12:8295c02d740f 129 // Make a painting movement upwards
ronvbree 12:8295c02d740f 130 void RobotController::paintUp() {
ronvbree 12:8295c02d740f 131 clearFlags();
ronvbree 12:8295c02d740f 132 float x,y;
ronvbree 12:8295c02d740f 133 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 134 referenceX = x;
ronvbree 12:8295c02d740f 135 referenceY = y;
ronvbree 0:494acf21d3bc 136
ronvbree 12:8295c02d740f 137 PAINT_MOVE_UP = true;
ronvbree 12:8295c02d740f 138
ronvbree 12:8295c02d740f 139 }
ronvbree 0:494acf21d3bc 140
ronvbree 12:8295c02d740f 141 // Make a painting movement downwards
ronvbree 12:8295c02d740f 142 void RobotController::paintDown() {
ronvbree 12:8295c02d740f 143 clearFlags();
ronvbree 12:8295c02d740f 144 float x,y;
ronvbree 12:8295c02d740f 145 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 146 referenceX = x;
ronvbree 12:8295c02d740f 147 referenceY = y;
ronvbree 12:8295c02d740f 148
ronvbree 12:8295c02d740f 149 PAINT_MOVE_DOWN = true;
ronvbree 12:8295c02d740f 150
ronvbree 12:8295c02d740f 151 }
ronvbree 7:a80cb6b06320 152
ronvbree 12:8295c02d740f 153 // Move to max y
ronvbree 12:8295c02d740f 154 void RobotController::goToTop() {
ronvbree 12:8295c02d740f 155 float upper, lower;
ronvbree 12:8295c02d740f 156 getArmLengthsForRollerPosition(topX, topY, upper, lower);
ronvbree 12:8295c02d740f 157 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 158 }
ronvbree 7:a80cb6b06320 159
ronvbree 12:8295c02d740f 160 // Move to min y
ronvbree 12:8295c02d740f 161 void RobotController::goToBottom() {
ronvbree 12:8295c02d740f 162 float upper, lower;
ronvbree 12:8295c02d740f 163 getArmLengthsForRollerPosition(bottomX, bottomY, upper, lower);
ronvbree 12:8295c02d740f 164 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 165 }
ronvbree 12:8295c02d740f 166
ronvbree 12:8295c02d740f 167 // Set all movement flags to false
ronvbree 12:8295c02d740f 168 void RobotController::clearFlags() {
ronvbree 12:8295c02d740f 169 MOVE_BY_REFERENCE = false;
ronvbree 12:8295c02d740f 170 PAINT_MOVE_UP = false;
ronvbree 12:8295c02d740f 171 PAINT_MOVE_DOWN = false;
ronvbree 12:8295c02d740f 172 }
ronvbree 7:a80cb6b06320 173
ronvbree 7:a80cb6b06320 174 /*
ronvbree 7:a80cb6b06320 175 Robot queries
ronvbree 7:a80cb6b06320 176 */
ronvbree 7:a80cb6b06320 177
ronvbree 7:a80cb6b06320 178 float RobotController::getUpperArmLength() {
ronvbree 7:a80cb6b06320 179 return robot.getUpperArmLength();
ronvbree 7:a80cb6b06320 180 }
ronvbree 7:a80cb6b06320 181
ronvbree 7:a80cb6b06320 182 float RobotController::getLowerArmLength() {
ronvbree 7:a80cb6b06320 183 return robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 184 }
ronvbree 7:a80cb6b06320 185
ronvbree 7:a80cb6b06320 186 bool RobotController::isKilled() {
ronvbree 7:a80cb6b06320 187 return robot.isKilled();
ronvbree 7:a80cb6b06320 188 }
ronvbree 7:a80cb6b06320 189
ronvbree 12:8295c02d740f 190 // Get the robot instance
ronvbree 7:a80cb6b06320 191 Robot* RobotController::getRobot() {
ronvbree 7:a80cb6b06320 192 return &robot;
ronvbree 7:a80cb6b06320 193 }
ronvbree 7:a80cb6b06320 194
ronvbree 7:a80cb6b06320 195