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@28:09b08753498c, 2016-11-03 (annotated)

Committer:
Jankoekenpan
Date:
Thu Nov 03 18:23:51 2016 +0000
Revision:
28:09b08753498c
Parent:
27:91dc5174333a
Parent:
25:bdb854127c11 
merge rons comments with my demo version

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
Jankoekenpan 28:09b08753498c 92
ronvbree 18:1c9dc6caab9d 93 // Instruct arm motors
ronvbree 18:1c9dc6caab9d 94 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 95 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 96
ronvbree 7:a80cb6b06320 97 }
ronvbree 7:a80cb6b06320 98
ronvbree 25:bdb854127c11 99 /*
ronvbree 25:bdb854127c11 100 PAINT MOVE DOWN
ronvbree 25:bdb854127c11 101
ronvbree 25:bdb854127c11 102 Very similar to paint move up.^
ronvbree 25:bdb854127c11 103 */
ronvbree 7:a80cb6b06320 104 if (PAINT_MOVE_DOWN) {
ronvbree 12:8295c02d740f 105 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 106 float x,y;
ronvbree 12:8295c02d740f 107 float upperArmLength = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 108 float lowerArmLength = robot.getLowerArmLength();
ronvbree 12:8295c02d740f 109 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 110
ronvbree 12:8295c02d740f 111 // Get controller output for leading arm (upper arm)
ronvbree 12:8295c02d740f 112 float yError = bottomY - y;
ronvbree 12:8295c02d740f 113 float upperOutput = -1 * upperArmController.control(yError, tick);
ronvbree 12:8295c02d740f 114
ronvbree 12:8295c02d740f 115 // Predict influence on x position
ronvbree 18:1c9dc6caab9d 116 float futureUpperArmLength = upperArmLength + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
ronvbree 12:8295c02d740f 117 float futureX, futureY;
ronvbree 18:1c9dc6caab9d 118 getRollerPositionForArmLengths(futureUpperArmLength, lowerArmLength, futureX, futureY);
ronvbree 12:8295c02d740f 119
ronvbree 12:8295c02d740f 120 // Calculate lower arm length that is needed to compensate for the leading arm
ronvbree 12:8295c02d740f 121 float upper,lower;
ronvbree 18:1c9dc6caab9d 122 getArmLengthsForRollerPosition(bottomX, futureY, upper, lower);
ronvbree 12:8295c02d740f 123
ronvbree 12:8295c02d740f 124 // Calculate compensating arm velocity based on error
ronvbree 12:8295c02d740f 125 float compensatingArmError = lower - lowerArmLength;
ronvbree 12:8295c02d740f 126 float lowerOutput = lowerXController.control(compensatingArmError, tick);
ronvbree 12:8295c02d740f 127
ronvbree 18:1c9dc6caab9d 128 // Instruct arm motors
ronvbree 12:8295c02d740f 129 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 130 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 131
ronvbree 0:494acf21d3bc 132 }
ronvbree 0:494acf21d3bc 133 }
ronvbree 0:494acf21d3bc 134
ronvbree 7:a80cb6b06320 135 /*
ronvbree 7:a80cb6b06320 136 Public methods
ronvbree 7:a80cb6b06320 137 */
ronvbree 7:a80cb6b06320 138
ronvbree 7:a80cb6b06320 139
ronvbree 7:a80cb6b06320 140 // Set the reference lengths for the arms that correspond to this roller coordinate
ronvbree 7:a80cb6b06320 141 void RobotController::moveTo(float x, float y) {
ronvbree 12:8295c02d740f 142 clearFlags();
ronvbree 7:a80cb6b06320 143 float upper, lower;
ronvbree 7:a80cb6b06320 144 getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference
ronvbree 7:a80cb6b06320 145 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 146 lowerArmLengthReference = lower;
ronvbree 12:8295c02d740f 147
ronvbree 7:a80cb6b06320 148 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 149 }
ronvbree 7:a80cb6b06320 150
ronvbree 7:a80cb6b06320 151 // Set the arm lengths
ronvbree 7:a80cb6b06320 152 void RobotController::setArmLengths(float upper, float lower) {
ronvbree 12:8295c02d740f 153 clearFlags();
ronvbree 12:8295c02d740f 154
ronvbree 7:a80cb6b06320 155 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 156 lowerArmLengthReference = lower;
ronvbree 7:a80cb6b06320 157
ronvbree 7:a80cb6b06320 158 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 159 }
ronvbree 7:a80cb6b06320 160
ronvbree 12:8295c02d740f 161 // Make a painting movement upwards
ronvbree 12:8295c02d740f 162 void RobotController::paintUp() {
ronvbree 12:8295c02d740f 163 clearFlags();
ronvbree 12:8295c02d740f 164 float x,y;
ronvbree 12:8295c02d740f 165 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 166 referenceX = x;
ronvbree 12:8295c02d740f 167 referenceY = y;
ronvbree 0:494acf21d3bc 168
ronvbree 12:8295c02d740f 169 PAINT_MOVE_UP = true;
ronvbree 12:8295c02d740f 170
ronvbree 12:8295c02d740f 171 }
ronvbree 0:494acf21d3bc 172
ronvbree 12:8295c02d740f 173 // Make a painting movement downwards
ronvbree 12:8295c02d740f 174 void RobotController::paintDown() {
ronvbree 12:8295c02d740f 175 clearFlags();
ronvbree 12:8295c02d740f 176 float x,y;
ronvbree 12:8295c02d740f 177 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 178 referenceX = x;
ronvbree 12:8295c02d740f 179 referenceY = y;
ronvbree 12:8295c02d740f 180
ronvbree 12:8295c02d740f 181 PAINT_MOVE_DOWN = true;
ronvbree 12:8295c02d740f 182
ronvbree 12:8295c02d740f 183 }
ronvbree 7:a80cb6b06320 184
ronvbree 12:8295c02d740f 185 // Move to max y
ronvbree 12:8295c02d740f 186 void RobotController::goToTop() {
ronvbree 12:8295c02d740f 187 float upper, lower;
ronvbree 12:8295c02d740f 188 getArmLengthsForRollerPosition(topX, topY, upper, lower);
ronvbree 12:8295c02d740f 189 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 190 }
ronvbree 7:a80cb6b06320 191
ronvbree 12:8295c02d740f 192 // Move to min y
ronvbree 12:8295c02d740f 193 void RobotController::goToBottom() {
ronvbree 12:8295c02d740f 194 float upper, lower;
ronvbree 12:8295c02d740f 195 getArmLengthsForRollerPosition(bottomX, bottomY, upper, lower);
ronvbree 12:8295c02d740f 196 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 197 }
ronvbree 12:8295c02d740f 198
ronvbree 12:8295c02d740f 199 // Set all movement flags to false
ronvbree 12:8295c02d740f 200 void RobotController::clearFlags() {
ronvbree 12:8295c02d740f 201 MOVE_BY_REFERENCE = false;
ronvbree 12:8295c02d740f 202 PAINT_MOVE_UP = false;
ronvbree 12:8295c02d740f 203 PAINT_MOVE_DOWN = false;
ronvbree 12:8295c02d740f 204 }
ronvbree 7:a80cb6b06320 205
ronvbree 7:a80cb6b06320 206 /*
ronvbree 7:a80cb6b06320 207 Robot queries
ronvbree 7:a80cb6b06320 208 */
ronvbree 7:a80cb6b06320 209
ronvbree 7:a80cb6b06320 210 float RobotController::getUpperArmLength() {
ronvbree 7:a80cb6b06320 211 return robot.getUpperArmLength();
ronvbree 7:a80cb6b06320 212 }
ronvbree 7:a80cb6b06320 213
ronvbree 7:a80cb6b06320 214 float RobotController::getLowerArmLength() {
ronvbree 7:a80cb6b06320 215 return robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 216 }
ronvbree 7:a80cb6b06320 217
ronvbree 7:a80cb6b06320 218 bool RobotController::isKilled() {
ronvbree 7:a80cb6b06320 219 return robot.isKilled();
ronvbree 7:a80cb6b06320 220 }
ronvbree 7:a80cb6b06320 221
ronvbree 12:8295c02d740f 222 // Get the robot instance
ronvbree 7:a80cb6b06320 223 Robot* RobotController::getRobot() {
ronvbree 7:a80cb6b06320 224 return &robot;
ronvbree 7:a80cb6b06320 225 }
ronvbree 7:a80cb6b06320 226
ronvbree 7:a80cb6b06320 227