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@25:bdb854127c11, 2016-11-03 (annotated)

Committer:
ronvbree
Date:
Thu Nov 03 17:47:39 2016 +0000
Revision:
25:bdb854127c11
Parent:
18:1c9dc6caab9d
Child:
28:09b08753498c
comments

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 18:1c9dc6caab9d 10 upperXController(1.2, 0, 0.03),
ronvbree 18:1c9dc6caab9d 11 lowerXController(0.6, 0, 0.03),
ronvbree 7:a80cb6b06320 12 debug(USBTX, USBRX),
ronvbree 7:a80cb6b06320 13 robot() {
ronvbree 12:8295c02d740f 14
ronvbree 12:8295c02d740f 15 // Set initial arm length as reference value
ronvbree 12:8295c02d740f 16 upperArmLengthReference = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 17 lowerArmLengthReference = robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 18
ronvbree 12:8295c02d740f 19 // Set initial roller position as reference value
ronvbree 12:8295c02d740f 20 float x,y; // Can't pass volatiles by reference
ronvbree 12:8295c02d740f 21 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 22 referenceX = x;
ronvbree 12:8295c02d740f 23 referenceY = y;
ronvbree 12:8295c02d740f 24
ronvbree 12:8295c02d740f 25 // Set movement flags
ronvbree 7:a80cb6b06320 26 MOVE_BY_REFERENCE = false;
ronvbree 7:a80cb6b06320 27 PAINT_MOVE_UP = false;
ronvbree 7:a80cb6b06320 28 PAINT_MOVE_DOWN = false;
ronvbree 7:a80cb6b06320 29
ronvbree 12:8295c02d740f 30 // Start ticker
ronvbree 7:a80cb6b06320 31 ticker.attach(this, &RobotController::doTick, tick);
ronvbree 7:a80cb6b06320 32
ronvbree 7:a80cb6b06320 33 }
ronvbree 7:a80cb6b06320 34
ronvbree 7:a80cb6b06320 35 /*
ronvbree 7:a80cb6b06320 36 Private Methods
ronvbree 7:a80cb6b06320 37 */
ronvbree 7:a80cb6b06320 38
ronvbree 7:a80cb6b06320 39 void RobotController::doTick() {
ronvbree 7:a80cb6b06320 40 robot.update();
ronvbree 7:a80cb6b06320 41
ronvbree 25:bdb854127c11 42 /*
ronvbree 25:bdb854127c11 43 MOVE BY REFERENCE
ronvbree 25:bdb854127c11 44
ronvbree 25:bdb854127c11 45 Movement is done based on the upperArmLengthReference and lowerArmLengthReference instance variables. An error between the current (estimated) arm
ronvbree 25:bdb854127c11 46 length and reference arm length is used to generate a proportional velocity to remove the error.
ronvbree 25:bdb854127c11 47 */
ronvbree 7:a80cb6b06320 48 if (MOVE_BY_REFERENCE) {
ronvbree 25:bdb854127c11 49 // Generate control velocities (rad/s)
ronvbree 7:a80cb6b06320 50 float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick);
ronvbree 7:a80cb6b06320 51 float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick);
ronvbree 25:bdb854127c11 52 // Instruct motors
ronvbree 7:a80cb6b06320 53 robot.setUpperArmVelocity(upperOutput);
ronvbree 7:a80cb6b06320 54 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 55 }
ronvbree 25:bdb854127c11 56 /*
ronvbree 25:bdb854127c11 57 PAINT MOVE UP
ronvbree 25:bdb854127c11 58
ronvbree 25:bdb854127c11 59 Painting movement makes a distinction between the two arms. One arm, which is referred to as the leading arm, controls the y value of the roller.
ronvbree 25:bdb854127c11 60 Painting upwards will require an increase in the y-coordinate of the roller. The velocity of the leading arm is proportional to the error in y
ronvbree 25:bdb854127c11 61 values.
ronvbree 25:bdb854127c11 62 The leading arm will, by altering the height of the roller, also influence the x coordinate of the roller. The other arm will compensate for this.
ronvbree 25:bdb854127c11 63 A prediction is made where the other arm will be the next tick (based on the arm's encoder velocity). This is used to make a prediction about the
ronvbree 25:bdb854127c11 64 location of the roller and which arm length is needed to compensate for the error of x. A PD-Controller then outputs a velocity to the compensating
ronvbree 25:bdb854127c11 65 arm, based on the length which is required to set x to its desired value.
ronvbree 25:bdb854127c11 66
ronvbree 25:bdb854127c11 67 */
ronvbree 7:a80cb6b06320 68 if (PAINT_MOVE_UP) {
ronvbree 18:1c9dc6caab9d 69 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 70 float x,y;
ronvbree 18:1c9dc6caab9d 71 float upperArmLength = robot.getUpperArmLength();
ronvbree 18:1c9dc6caab9d 72 float lowerArmLength = robot.getLowerArmLength();
ronvbree 18:1c9dc6caab9d 73 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 74
ronvbree 18:1c9dc6caab9d 75 // Get controller output for leading arm (lower arm)
ronvbree 12:8295c02d740f 76 float yError = topY - y;
ronvbree 12:8295c02d740f 77 float lowerOutput = lowerArmController.control(yError, tick);
ronvbree 12:8295c02d740f 78
ronvbree 18:1c9dc6caab9d 79 // Predict influence on x position
ronvbree 18:1c9dc6caab9d 80 float futureLowerArmLength = lowerArmLength + robot.getLowerArmEncoderVelocity() * gearRadius * tick;
ronvbree 18:1c9dc6caab9d 81 float futureX, futureY;
ronvbree 18:1c9dc6caab9d 82 getRollerPositionForArmLengths(upperArmLength, futureLowerArmLength, futureX, futureY);
ronvbree 18:1c9dc6caab9d 83
ronvbree 18:1c9dc6caab9d 84 // Calculate upper arm length that is needed to compensate for the leading arm
ronvbree 18:1c9dc6caab9d 85 float upper,lower;
ronvbree 18:1c9dc6caab9d 86 getArmLengthsForRollerPosition(topX, futureY, upper, lower);
ronvbree 18:1c9dc6caab9d 87
ronvbree 18:1c9dc6caab9d 88 // Calculate compensating arm velocity based on error
ronvbree 18:1c9dc6caab9d 89 float compensatingArmError = upper - upperArmLength;
ronvbree 18:1c9dc6caab9d 90 float upperOutput = upperXController.control(compensatingArmError, tick);
ronvbree 18:1c9dc6caab9d 91
ronvbree 18:1c9dc6caab9d 92 // Instruct arm motors
ronvbree 18:1c9dc6caab9d 93 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 94 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 95
ronvbree 7:a80cb6b06320 96 }
ronvbree 7:a80cb6b06320 97
ronvbree 25:bdb854127c11 98 /*
ronvbree 25:bdb854127c11 99 PAINT MOVE DOWN
ronvbree 25:bdb854127c11 100
ronvbree 25:bdb854127c11 101 Very similar to paint move up.^
ronvbree 25:bdb854127c11 102 */
ronvbree 7:a80cb6b06320 103 if (PAINT_MOVE_DOWN) {
ronvbree 12:8295c02d740f 104 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 105 float x,y;
ronvbree 12:8295c02d740f 106 float upperArmLength = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 107 float lowerArmLength = robot.getLowerArmLength();
ronvbree 12:8295c02d740f 108 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 109
ronvbree 12:8295c02d740f 110 // Get controller output for leading arm (upper arm)
ronvbree 12:8295c02d740f 111 float yError = bottomY - y;
ronvbree 12:8295c02d740f 112 float upperOutput = -1 * upperArmController.control(yError, tick);
ronvbree 12:8295c02d740f 113
ronvbree 12:8295c02d740f 114 // Predict influence on x position
ronvbree 18:1c9dc6caab9d 115 float futureUpperArmLength = upperArmLength + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
ronvbree 12:8295c02d740f 116 float futureX, futureY;
ronvbree 18:1c9dc6caab9d 117 getRollerPositionForArmLengths(futureUpperArmLength, lowerArmLength, futureX, futureY);
ronvbree 12:8295c02d740f 118
ronvbree 12:8295c02d740f 119 // Calculate lower arm length that is needed to compensate for the leading arm
ronvbree 12:8295c02d740f 120 float upper,lower;
ronvbree 18:1c9dc6caab9d 121 getArmLengthsForRollerPosition(bottomX, futureY, upper, lower);
ronvbree 12:8295c02d740f 122
ronvbree 12:8295c02d740f 123 // Calculate compensating arm velocity based on error
ronvbree 12:8295c02d740f 124 float compensatingArmError = lower - lowerArmLength;
ronvbree 12:8295c02d740f 125 float lowerOutput = lowerXController.control(compensatingArmError, tick);
ronvbree 12:8295c02d740f 126
ronvbree 18:1c9dc6caab9d 127 // Instruct arm motors
ronvbree 12:8295c02d740f 128 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 129 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 130
ronvbree 0:494acf21d3bc 131 }
ronvbree 0:494acf21d3bc 132 }
ronvbree 0:494acf21d3bc 133
ronvbree 7:a80cb6b06320 134 /*
ronvbree 7:a80cb6b06320 135 Public methods
ronvbree 7:a80cb6b06320 136 */
ronvbree 7:a80cb6b06320 137
ronvbree 7:a80cb6b06320 138
ronvbree 7:a80cb6b06320 139 // Set the reference lengths for the arms that correspond to this roller coordinate
ronvbree 7:a80cb6b06320 140 void RobotController::moveTo(float x, float y) {
ronvbree 12:8295c02d740f 141 clearFlags();
ronvbree 7:a80cb6b06320 142 float upper, lower;
ronvbree 7:a80cb6b06320 143 getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference
ronvbree 7:a80cb6b06320 144 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 145 lowerArmLengthReference = lower;
ronvbree 12:8295c02d740f 146
ronvbree 7:a80cb6b06320 147 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 148 }
ronvbree 7:a80cb6b06320 149
ronvbree 7:a80cb6b06320 150 // Set the arm lengths
ronvbree 7:a80cb6b06320 151 void RobotController::setArmLengths(float upper, float lower) {
ronvbree 12:8295c02d740f 152 clearFlags();
ronvbree 12:8295c02d740f 153
ronvbree 7:a80cb6b06320 154 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 155 lowerArmLengthReference = lower;
ronvbree 7:a80cb6b06320 156
ronvbree 7:a80cb6b06320 157 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 158 }
ronvbree 7:a80cb6b06320 159
ronvbree 12:8295c02d740f 160 // Make a painting movement upwards
ronvbree 12:8295c02d740f 161 void RobotController::paintUp() {
ronvbree 12:8295c02d740f 162 clearFlags();
ronvbree 12:8295c02d740f 163 float x,y;
ronvbree 12:8295c02d740f 164 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 165 referenceX = x;
ronvbree 12:8295c02d740f 166 referenceY = y;
ronvbree 0:494acf21d3bc 167
ronvbree 12:8295c02d740f 168 PAINT_MOVE_UP = true;
ronvbree 12:8295c02d740f 169
ronvbree 12:8295c02d740f 170 }
ronvbree 0:494acf21d3bc 171
ronvbree 12:8295c02d740f 172 // Make a painting movement downwards
ronvbree 12:8295c02d740f 173 void RobotController::paintDown() {
ronvbree 12:8295c02d740f 174 clearFlags();
ronvbree 12:8295c02d740f 175 float x,y;
ronvbree 12:8295c02d740f 176 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 177 referenceX = x;
ronvbree 12:8295c02d740f 178 referenceY = y;
ronvbree 12:8295c02d740f 179
ronvbree 12:8295c02d740f 180 PAINT_MOVE_DOWN = true;
ronvbree 12:8295c02d740f 181
ronvbree 12:8295c02d740f 182 }
ronvbree 7:a80cb6b06320 183
ronvbree 12:8295c02d740f 184 // Move to max y
ronvbree 12:8295c02d740f 185 void RobotController::goToTop() {
ronvbree 12:8295c02d740f 186 float upper, lower;
ronvbree 12:8295c02d740f 187 getArmLengthsForRollerPosition(topX, topY, upper, lower);
ronvbree 12:8295c02d740f 188 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 189 }
ronvbree 7:a80cb6b06320 190
ronvbree 12:8295c02d740f 191 // Move to min y
ronvbree 12:8295c02d740f 192 void RobotController::goToBottom() {
ronvbree 12:8295c02d740f 193 float upper, lower;
ronvbree 12:8295c02d740f 194 getArmLengthsForRollerPosition(bottomX, bottomY, upper, lower);
ronvbree 12:8295c02d740f 195 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 196 }
ronvbree 12:8295c02d740f 197
ronvbree 12:8295c02d740f 198 // Set all movement flags to false
ronvbree 12:8295c02d740f 199 void RobotController::clearFlags() {
ronvbree 12:8295c02d740f 200 MOVE_BY_REFERENCE = false;
ronvbree 12:8295c02d740f 201 PAINT_MOVE_UP = false;
ronvbree 12:8295c02d740f 202 PAINT_MOVE_DOWN = false;
ronvbree 12:8295c02d740f 203 }
ronvbree 7:a80cb6b06320 204
ronvbree 7:a80cb6b06320 205 /*
ronvbree 7:a80cb6b06320 206 Robot queries
ronvbree 7:a80cb6b06320 207 */
ronvbree 7:a80cb6b06320 208
ronvbree 7:a80cb6b06320 209 float RobotController::getUpperArmLength() {
ronvbree 7:a80cb6b06320 210 return robot.getUpperArmLength();
ronvbree 7:a80cb6b06320 211 }
ronvbree 7:a80cb6b06320 212
ronvbree 7:a80cb6b06320 213 float RobotController::getLowerArmLength() {
ronvbree 7:a80cb6b06320 214 return robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 215 }
ronvbree 7:a80cb6b06320 216
ronvbree 7:a80cb6b06320 217 bool RobotController::isKilled() {
ronvbree 7:a80cb6b06320 218 return robot.isKilled();
ronvbree 7:a80cb6b06320 219 }
ronvbree 7:a80cb6b06320 220
ronvbree 12:8295c02d740f 221 // Get the robot instance
ronvbree 7:a80cb6b06320 222 Robot* RobotController::getRobot() {
ronvbree 7:a80cb6b06320 223 return &robot;
ronvbree 7:a80cb6b06320 224 }
ronvbree 7:a80cb6b06320 225
ronvbree 7:a80cb6b06320 226