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

robot.cpp@2:fc869e45e672, 2016-11-02 (annotated)

Committer:
ronvbree
Date:
Wed Nov 02 08:51:12 2016 +0000
Revision:
2:fc869e45e672
Child:
3:1f47375270c5
abcdg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ronvbree 2:fc869e45e672 1 #include "robot.h"
ronvbree 2:fc869e45e672 2 #include "geometry.h"
ronvbree 2:fc869e45e672 3
ronvbree 2:fc869e45e672 4 /*
ronvbree 2:fc869e45e672 5 Pins
ronvbree 2:fc869e45e672 6 */
ronvbree 2:fc869e45e672 7
ronvbree 2:fc869e45e672 8 const PinName upperMotorDir = D7;
ronvbree 2:fc869e45e672 9 const PinName upperMotorPWM = D6;
ronvbree 2:fc869e45e672 10
ronvbree 2:fc869e45e672 11 const PinName lowerMotorDir = D4;
ronvbree 2:fc869e45e672 12 const PinName lowerMotorPWM = D5;
ronvbree 2:fc869e45e672 13
ronvbree 2:fc869e45e672 14 const PinName upperEncoderPin1 = D13;
ronvbree 2:fc869e45e672 15 const PinName upperEncoderPin2 = D12;
ronvbree 2:fc869e45e672 16
ronvbree 2:fc869e45e672 17 const PinName lowerEncoderPin1 = D10;
ronvbree 2:fc869e45e672 18 const PinName lowerEncoderPin2 = D9;
ronvbree 2:fc869e45e672 19
ronvbree 2:fc869e45e672 20 const PinName buttonPin = D3;
ronvbree 2:fc869e45e672 21
ronvbree 2:fc869e45e672 22 /*
ronvbree 2:fc869e45e672 23 Constructor
ronvbree 2:fc869e45e672 24 */
ronvbree 2:fc869e45e672 25
ronvbree 2:fc869e45e672 26 Robot::Robot():
ronvbree 2:fc869e45e672 27 upperArm(L_min, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
ronvbree 2:fc869e45e672 28 lowerArm(L_min, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2),
ronvbree 2:fc869e45e672 29 killButton(buttonPin){
ronvbree 2:fc869e45e672 30 killButton.fall(this, &Robot::kill);
ronvbree 2:fc869e45e672 31 ticker.attach(this, &Robot::doTick, tick);
ronvbree 2:fc869e45e672 32 // x,y coordinate when both arms are of length L_min
ronvbree 2:fc869e45e672 33 y = 0.5 * d + h;
ronvbree 2:fc869e45e672 34 x = sqrt(pow(L_min,2) - pow(0.5 * d, 2));
ronvbree 2:fc869e45e672 35
ronvbree 2:fc869e45e672 36 }
ronvbree 2:fc869e45e672 37
ronvbree 2:fc869e45e672 38 /*
ronvbree 2:fc869e45e672 39 Methods
ronvbree 2:fc869e45e672 40 */
ronvbree 2:fc869e45e672 41
ronvbree 2:fc869e45e672 42 void Robot::doTick() {
ronvbree 2:fc869e45e672 43 upperArm.update();
ronvbree 2:fc869e45e672 44 lowerArm.update();
ronvbree 2:fc869e45e672 45
ronvbree 2:fc869e45e672 46 }
ronvbree 2:fc869e45e672 47
ronvbree 2:fc869e45e672 48 void Robot::moveY(float dy) {
ronvbree 2:fc869e45e672 49 float goal = y + dy;
ronvbree 2:fc869e45e672 50 float upperArmLengthGoal;
ronvbree 2:fc869e45e672 51 float lowerArmLengthGoal;
ronvbree 2:fc869e45e672 52
ronvbree 2:fc869e45e672 53 const float n = 10;
ronvbree 2:fc869e45e672 54 float dy_step = dy/n;
ronvbree 2:fc869e45e672 55
ronvbree 2:fc869e45e672 56 }
ronvbree 2:fc869e45e672 57
ronvbree 2:fc869e45e672 58 // Set upper arm velocity (rad/s)
ronvbree 2:fc869e45e672 59 void Robot::setUpperArmVelocity(float referenceVelocity) {
ronvbree 2:fc869e45e672 60 upperArm.setVelocity(referenceVelocity);
ronvbree 2:fc869e45e672 61 }
ronvbree 2:fc869e45e672 62
ronvbree 2:fc869e45e672 63 // Set lower arm velocity (rad/s) (multiplied with -1 because for some reason the directions are different for the two arms)
ronvbree 2:fc869e45e672 64 void Robot::setLowerArmVelocity(float referenceVelocity) {
ronvbree 2:fc869e45e672 65 lowerArm.setVelocity(-1 * referenceVelocity);
ronvbree 2:fc869e45e672 66 }
ronvbree 2:fc869e45e672 67
ronvbree 2:fc869e45e672 68 //void Robot::setArms(float upper, float lower) {
ronvbree 2:fc869e45e672 69 // const float referenceVelocity = 1;
ronvbree 2:fc869e45e672 70 //
ronvbree 2:fc869e45e672 71 // bool upperGoalReached = false;
ronvbree 2:fc869e45e672 72 // bool lowerGoalReached = false;
ronvbree 2:fc869e45e672 73 //
ronvbree 2:fc869e45e672 74 // float d_upper = upper-upperArm.getLength();
ronvbree 2:fc869e45e672 75 // float d_lower = lower-lowerArm.getLength();
ronvbree 2:fc869e45e672 76 //
ronvbree 2:fc869e45e672 77 // bool upperDir = d_upper >= 0;
ronvbree 2:fc869e45e672 78 // bool lowerDir = d_lower >= 0;
ronvbree 2:fc869e45e672 79 //
ronvbree 2:fc869e45e672 80 // setUpperArmVelocity(upperDir? -1*referenceVelocity:referenceVelocity);
ronvbree 2:fc869e45e672 81 // setLowerArmVelocity(lowerDir? referenceVelocity:-1*referenceVelocity);
ronvbree 2:fc869e45e672 82 //
ronvbree 2:fc869e45e672 83 // while (!(upperGoalReached && lowerGoalReached)) {
ronvbree 2:fc869e45e672 84 // if (!upperGoalReached) {
ronvbree 2:fc869e45e672 85 // d_upper = upper-upperArm.getLength();
ronvbree 2:fc869e45e672 86 // if (upperDir) {
ronvbree 2:fc869e45e672 87 // upperGoalReached = d_upper < 0;
ronvbree 2:fc869e45e672 88 // } else {
ronvbree 2:fc869e45e672 89 // upperGoalReached = d_upper >= 0;
ronvbree 2:fc869e45e672 90 // }
ronvbree 2:fc869e45e672 91 // if (upperGoalReached) {
ronvbree 2:fc869e45e672 92 // setUpperArmVelocity(0);
ronvbree 2:fc869e45e672 93 // }
ronvbree 2:fc869e45e672 94 // }
ronvbree 2:fc869e45e672 95 // if (!lowerGoalReached) {
ronvbree 2:fc869e45e672 96 // d_lower = lower-lowerArm.getLength();
ronvbree 2:fc869e45e672 97 // if (lowerDir) {
ronvbree 2:fc869e45e672 98 // lowerGoalReached = d_lower < 0;
ronvbree 2:fc869e45e672 99 // } else {
ronvbree 2:fc869e45e672 100 // lowerGoalReached = d_lower >= 0;
ronvbree 2:fc869e45e672 101 // }
ronvbree 2:fc869e45e672 102 // if (lowerGoalReached) {
ronvbree 2:fc869e45e672 103 // setLowerArmVelocity(0);
ronvbree 2:fc869e45e672 104 // }
ronvbree 2:fc869e45e672 105 // }
ronvbree 2:fc869e45e672 106 // }
ronvbree 2:fc869e45e672 107 //
ronvbree 2:fc869e45e672 108 // // TODO -- update x/y values?
ronvbree 2:fc869e45e672 109 //
ronvbree 2:fc869e45e672 110 //}
ronvbree 2:fc869e45e672 111
ronvbree 2:fc869e45e672 112 // Turn off both arms
ronvbree 2:fc869e45e672 113 void Robot::kill() {
ronvbree 2:fc869e45e672 114 upperArm.kill();
ronvbree 2:fc869e45e672 115 lowerArm.kill();
ronvbree 2:fc869e45e672 116 }