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@18:1c9dc6caab9d, 2016-11-03 (annotated)

Committer:
ronvbree
Date:
Thu Nov 03 16:29:31 2016 +0000
Revision:
18:1c9dc6caab9d
Parent:
14:551049a798a3
Child:
25:bdb854127c11
Child:
27:91dc5174333a
final

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 7:a80cb6b06320 42 if (MOVE_BY_REFERENCE) {
ronvbree 7:a80cb6b06320 43 float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick);
ronvbree 7:a80cb6b06320 44 float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick);
ronvbree 7:a80cb6b06320 45
ronvbree 7:a80cb6b06320 46 robot.setUpperArmVelocity(upperOutput);
ronvbree 7:a80cb6b06320 47 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 48 }
ronvbree 0:494acf21d3bc 49
ronvbree 7:a80cb6b06320 50 if (PAINT_MOVE_UP) {
ronvbree 18:1c9dc6caab9d 51 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 52 float x,y;
ronvbree 18:1c9dc6caab9d 53 float upperArmLength = robot.getUpperArmLength();
ronvbree 18:1c9dc6caab9d 54 float lowerArmLength = robot.getLowerArmLength();
ronvbree 18:1c9dc6caab9d 55 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 56
ronvbree 18:1c9dc6caab9d 57 // Get controller output for leading arm (lower arm)
ronvbree 12:8295c02d740f 58 float yError = topY - y;
ronvbree 12:8295c02d740f 59 float lowerOutput = lowerArmController.control(yError, tick);
ronvbree 12:8295c02d740f 60
ronvbree 18:1c9dc6caab9d 61 // Predict influence on x position
ronvbree 18:1c9dc6caab9d 62 float futureLowerArmLength = lowerArmLength + robot.getLowerArmEncoderVelocity() * gearRadius * tick;
ronvbree 18:1c9dc6caab9d 63 float futureX, futureY;
ronvbree 18:1c9dc6caab9d 64 getRollerPositionForArmLengths(upperArmLength, futureLowerArmLength, futureX, futureY);
ronvbree 18:1c9dc6caab9d 65
ronvbree 18:1c9dc6caab9d 66 // Calculate upper arm length that is needed to compensate for the leading arm
ronvbree 18:1c9dc6caab9d 67 float upper,lower;
ronvbree 18:1c9dc6caab9d 68 getArmLengthsForRollerPosition(topX, futureY, upper, lower);
ronvbree 18:1c9dc6caab9d 69
ronvbree 18:1c9dc6caab9d 70 // Calculate compensating arm velocity based on error
ronvbree 18:1c9dc6caab9d 71 float compensatingArmError = upper - upperArmLength;
ronvbree 18:1c9dc6caab9d 72 debug.printf("Upper Arm error: %f\n\r", compensatingArmError);
ronvbree 18:1c9dc6caab9d 73
ronvbree 18:1c9dc6caab9d 74 float upperOutput = upperXController.control(compensatingArmError, tick);
ronvbree 18:1c9dc6caab9d 75
ronvbree 18:1c9dc6caab9d 76 debug.printf("Resulting reference velocity: %f\n\r", upperOutput);
ronvbree 18:1c9dc6caab9d 77
ronvbree 18:1c9dc6caab9d 78 // Instruct arm motors
ronvbree 18:1c9dc6caab9d 79 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 80 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 81
ronvbree 7:a80cb6b06320 82 }
ronvbree 7:a80cb6b06320 83
ronvbree 7:a80cb6b06320 84 if (PAINT_MOVE_DOWN) {
ronvbree 12:8295c02d740f 85 // Get current roller location and arm lengths
ronvbree 12:8295c02d740f 86 float x,y;
ronvbree 12:8295c02d740f 87 float upperArmLength = robot.getUpperArmLength();
ronvbree 12:8295c02d740f 88 float lowerArmLength = robot.getLowerArmLength();
ronvbree 12:8295c02d740f 89 getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
ronvbree 12:8295c02d740f 90
ronvbree 12:8295c02d740f 91 // Get controller output for leading arm (upper arm)
ronvbree 12:8295c02d740f 92 float yError = bottomY - y;
ronvbree 12:8295c02d740f 93 float upperOutput = -1 * upperArmController.control(yError, tick);
ronvbree 12:8295c02d740f 94
ronvbree 12:8295c02d740f 95 // Predict influence on x position
ronvbree 18:1c9dc6caab9d 96 float futureUpperArmLength = upperArmLength + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
ronvbree 12:8295c02d740f 97 float futureX, futureY;
ronvbree 18:1c9dc6caab9d 98 getRollerPositionForArmLengths(futureUpperArmLength, lowerArmLength, futureX, futureY);
ronvbree 12:8295c02d740f 99
ronvbree 12:8295c02d740f 100 // Calculate lower arm length that is needed to compensate for the leading arm
ronvbree 12:8295c02d740f 101 float upper,lower;
ronvbree 18:1c9dc6caab9d 102 getArmLengthsForRollerPosition(bottomX, futureY, upper, lower);
ronvbree 12:8295c02d740f 103
ronvbree 12:8295c02d740f 104 // Calculate compensating arm velocity based on error
ronvbree 12:8295c02d740f 105 float compensatingArmError = lower - lowerArmLength;
ronvbree 12:8295c02d740f 106 float lowerOutput = lowerXController.control(compensatingArmError, tick);
ronvbree 12:8295c02d740f 107
ronvbree 18:1c9dc6caab9d 108 // Instruct arm motors
ronvbree 12:8295c02d740f 109 robot.setUpperArmVelocity(upperOutput);
ronvbree 12:8295c02d740f 110 robot.setLowerArmVelocity(lowerOutput);
ronvbree 7:a80cb6b06320 111
ronvbree 0:494acf21d3bc 112 }
ronvbree 0:494acf21d3bc 113 }
ronvbree 0:494acf21d3bc 114
ronvbree 7:a80cb6b06320 115 /*
ronvbree 7:a80cb6b06320 116 Public methods
ronvbree 7:a80cb6b06320 117 */
ronvbree 7:a80cb6b06320 118
ronvbree 7:a80cb6b06320 119
ronvbree 7:a80cb6b06320 120 // Set the reference lengths for the arms that correspond to this roller coordinate
ronvbree 7:a80cb6b06320 121 void RobotController::moveTo(float x, float y) {
ronvbree 12:8295c02d740f 122 clearFlags();
ronvbree 7:a80cb6b06320 123 float upper, lower;
ronvbree 7:a80cb6b06320 124 getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference
ronvbree 7:a80cb6b06320 125 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 126 lowerArmLengthReference = lower;
ronvbree 12:8295c02d740f 127
ronvbree 7:a80cb6b06320 128 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 129 }
ronvbree 7:a80cb6b06320 130
ronvbree 7:a80cb6b06320 131 // Set the arm lengths
ronvbree 7:a80cb6b06320 132 void RobotController::setArmLengths(float upper, float lower) {
ronvbree 12:8295c02d740f 133 clearFlags();
ronvbree 12:8295c02d740f 134
ronvbree 7:a80cb6b06320 135 upperArmLengthReference = upper;
ronvbree 7:a80cb6b06320 136 lowerArmLengthReference = lower;
ronvbree 7:a80cb6b06320 137
ronvbree 7:a80cb6b06320 138 MOVE_BY_REFERENCE = true;
ronvbree 7:a80cb6b06320 139
ronvbree 7:a80cb6b06320 140 debug.printf("Set arm length reference to: Upper %f, Lower %f\n\r", upper, lower);
ronvbree 7:a80cb6b06320 141 }
ronvbree 7:a80cb6b06320 142
ronvbree 12:8295c02d740f 143 // Make a painting movement upwards
ronvbree 12:8295c02d740f 144 void RobotController::paintUp() {
ronvbree 12:8295c02d740f 145 clearFlags();
ronvbree 12:8295c02d740f 146 float x,y;
ronvbree 12:8295c02d740f 147 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 148 referenceX = x;
ronvbree 12:8295c02d740f 149 referenceY = y;
ronvbree 0:494acf21d3bc 150
ronvbree 12:8295c02d740f 151 PAINT_MOVE_UP = true;
ronvbree 12:8295c02d740f 152
ronvbree 12:8295c02d740f 153 }
ronvbree 0:494acf21d3bc 154
ronvbree 12:8295c02d740f 155 // Make a painting movement downwards
ronvbree 12:8295c02d740f 156 void RobotController::paintDown() {
ronvbree 12:8295c02d740f 157 clearFlags();
ronvbree 12:8295c02d740f 158 float x,y;
ronvbree 12:8295c02d740f 159 getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
ronvbree 12:8295c02d740f 160 referenceX = x;
ronvbree 12:8295c02d740f 161 referenceY = y;
ronvbree 12:8295c02d740f 162
ronvbree 12:8295c02d740f 163 PAINT_MOVE_DOWN = true;
ronvbree 12:8295c02d740f 164
ronvbree 12:8295c02d740f 165 }
ronvbree 7:a80cb6b06320 166
ronvbree 12:8295c02d740f 167 // Move to max y
ronvbree 12:8295c02d740f 168 void RobotController::goToTop() {
ronvbree 12:8295c02d740f 169 float upper, lower;
ronvbree 12:8295c02d740f 170 getArmLengthsForRollerPosition(topX, topY, upper, lower);
ronvbree 12:8295c02d740f 171 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 172 }
ronvbree 7:a80cb6b06320 173
ronvbree 12:8295c02d740f 174 // Move to min y
ronvbree 12:8295c02d740f 175 void RobotController::goToBottom() {
ronvbree 12:8295c02d740f 176 float upper, lower;
ronvbree 12:8295c02d740f 177 getArmLengthsForRollerPosition(bottomX, bottomY, upper, lower);
ronvbree 12:8295c02d740f 178 setArmLengths(upper, lower);
ronvbree 12:8295c02d740f 179 }
ronvbree 12:8295c02d740f 180
ronvbree 12:8295c02d740f 181 // Set all movement flags to false
ronvbree 12:8295c02d740f 182 void RobotController::clearFlags() {
ronvbree 12:8295c02d740f 183 MOVE_BY_REFERENCE = false;
ronvbree 12:8295c02d740f 184 PAINT_MOVE_UP = false;
ronvbree 12:8295c02d740f 185 PAINT_MOVE_DOWN = false;
ronvbree 12:8295c02d740f 186 }
ronvbree 7:a80cb6b06320 187
ronvbree 7:a80cb6b06320 188 /*
ronvbree 7:a80cb6b06320 189 Robot queries
ronvbree 7:a80cb6b06320 190 */
ronvbree 7:a80cb6b06320 191
ronvbree 7:a80cb6b06320 192 float RobotController::getUpperArmLength() {
ronvbree 7:a80cb6b06320 193 return robot.getUpperArmLength();
ronvbree 7:a80cb6b06320 194 }
ronvbree 7:a80cb6b06320 195
ronvbree 7:a80cb6b06320 196 float RobotController::getLowerArmLength() {
ronvbree 7:a80cb6b06320 197 return robot.getLowerArmLength();
ronvbree 7:a80cb6b06320 198 }
ronvbree 7:a80cb6b06320 199
ronvbree 7:a80cb6b06320 200 bool RobotController::isKilled() {
ronvbree 7:a80cb6b06320 201 return robot.isKilled();
ronvbree 7:a80cb6b06320 202 }
ronvbree 7:a80cb6b06320 203
ronvbree 12:8295c02d740f 204 // Get the robot instance
ronvbree 7:a80cb6b06320 205 Robot* RobotController::getRobot() {
ronvbree 7:a80cb6b06320 206 return &robot;
ronvbree 7:a80cb6b06320 207 }
ronvbree 7:a80cb6b06320 208
ronvbree 7:a80cb6b06320 209