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@12:8295c02d740f, 2016-11-03 (annotated)

Committer:
ronvbree
Date:
Thu Nov 03 13:08:44 2016 +0000
Revision:
12:8295c02d740f
Parent:
7:a80cb6b06320
Child:
18:1c9dc6caab9d
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ronvbree 2:fc869e45e672 1 #include "robot.h"
ronvbree 2:fc869e45e672 2
ronvbree 2:fc869e45e672 3 /*
ronvbree 2:fc869e45e672 4 Pins
ronvbree 2:fc869e45e672 5 */
ronvbree 2:fc869e45e672 6
ronvbree 2:fc869e45e672 7 const PinName upperMotorDir = D7;
ronvbree 2:fc869e45e672 8 const PinName upperMotorPWM = D6;
ronvbree 2:fc869e45e672 9
ronvbree 2:fc869e45e672 10 const PinName lowerMotorDir = D4;
ronvbree 2:fc869e45e672 11 const PinName lowerMotorPWM = D5;
ronvbree 2:fc869e45e672 12
ronvbree 2:fc869e45e672 13 const PinName upperEncoderPin1 = D13;
ronvbree 2:fc869e45e672 14 const PinName upperEncoderPin2 = D12;
ronvbree 2:fc869e45e672 15
ronvbree 2:fc869e45e672 16 const PinName lowerEncoderPin1 = D10;
ronvbree 2:fc869e45e672 17 const PinName lowerEncoderPin2 = D9;
ronvbree 2:fc869e45e672 18
ronvbree 7:a80cb6b06320 19 const float initialArmLength = L_min;
ronvbree 2:fc869e45e672 20
ronvbree 12:8295c02d740f 21 const float maxUpperArmVelocity = 1; // Rad/s
ronvbree 12:8295c02d740f 22 const float maxLowerArmVelocity = 3; // Rad/s
ronvbree 12:8295c02d740f 23
ronvbree 2:fc869e45e672 24 /*
ronvbree 2:fc869e45e672 25 Constructor
ronvbree 2:fc869e45e672 26 */
ronvbree 2:fc869e45e672 27
ronvbree 2:fc869e45e672 28 Robot::Robot():
ronvbree 12:8295c02d740f 29 upperArm(initialArmLength, maxUpperArmVelocity, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
ronvbree 12:8295c02d740f 30 lowerArm(initialArmLength, maxLowerArmVelocity, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2) {
ronvbree 7:a80cb6b06320 31 killed = false;
ronvbree 2:fc869e45e672 32 }
ronvbree 2:fc869e45e672 33
ronvbree 2:fc869e45e672 34 /*
ronvbree 2:fc869e45e672 35 Methods
ronvbree 2:fc869e45e672 36 */
ronvbree 2:fc869e45e672 37
ronvbree 7:a80cb6b06320 38 // Update the arms
ronvbree 7:a80cb6b06320 39 void Robot::update() {
ronvbree 2:fc869e45e672 40 upperArm.update();
ronvbree 2:fc869e45e672 41 lowerArm.update();
ronvbree 2:fc869e45e672 42 }
ronvbree 2:fc869e45e672 43
ronvbree 12:8295c02d740f 44 // Get upper arm encoder velocity
ronvbree 12:8295c02d740f 45 float Robot::getUpperArmEncoderVelocity() {
ronvbree 12:8295c02d740f 46 return upperArm.getEncoderVelocity();
ronvbree 12:8295c02d740f 47 }
ronvbree 12:8295c02d740f 48
ronvbree 12:8295c02d740f 49 // Get lower arm encoder velocity
ronvbree 12:8295c02d740f 50 float Robot::getLowerArmEncoderVelocity() {
ronvbree 12:8295c02d740f 51 return lowerArm.getEncoderVelocity();
ronvbree 12:8295c02d740f 52 }
ronvbree 12:8295c02d740f 53
ronvbree 2:fc869e45e672 54 // Set upper arm velocity (rad/s)
ronvbree 2:fc869e45e672 55 void Robot::setUpperArmVelocity(float referenceVelocity) {
ronvbree 7:a80cb6b06320 56 if (!killed) {
ronvbree 7:a80cb6b06320 57 upperArm.setVelocity(referenceVelocity);
ronvbree 7:a80cb6b06320 58 }
ronvbree 3:1f47375270c5 59 }
ronvbree 3:1f47375270c5 60
ronvbree 2:fc869e45e672 61 // Set lower arm velocity (rad/s) (multiplied with -1 because for some reason the directions are different for the two arms)
ronvbree 2:fc869e45e672 62 void Robot::setLowerArmVelocity(float referenceVelocity) {
ronvbree 7:a80cb6b06320 63 if (!killed) {
ronvbree 7:a80cb6b06320 64 lowerArm.setVelocity(-1 * referenceVelocity);
ronvbree 7:a80cb6b06320 65 }
ronvbree 2:fc869e45e672 66 }
ronvbree 2:fc869e45e672 67
ronvbree 2:fc869e45e672 68 // Turn off both arms
ronvbree 2:fc869e45e672 69 void Robot::kill() {
ronvbree 7:a80cb6b06320 70 killed = true;
ronvbree 2:fc869e45e672 71 upperArm.kill();
ronvbree 2:fc869e45e672 72 lowerArm.kill();
ronvbree 2:fc869e45e672 73 }
ronvbree 7:a80cb6b06320 74
ronvbree 7:a80cb6b06320 75 // Indicates if the robot has been killed
ronvbree 7:a80cb6b06320 76 bool Robot::isKilled() {
ronvbree 7:a80cb6b06320 77 return killed;
ronvbree 7:a80cb6b06320 78 }
ronvbree 7:a80cb6b06320 79
ronvbree 7:a80cb6b06320 80 // Get upper arm length
ronvbree 7:a80cb6b06320 81 float Robot::getUpperArmLength(){
ronvbree 7:a80cb6b06320 82 return upperArm.getLength();
ronvbree 7:a80cb6b06320 83 }
ronvbree 7:a80cb6b06320 84
ronvbree 7:a80cb6b06320 85 // Get lower arm length
ronvbree 7:a80cb6b06320 86 float Robot::getLowerArmLength(){
ronvbree 7:a80cb6b06320 87 return lowerArm.getLength();
ronvbree 7:a80cb6b06320 88 }