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

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

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 25:bdb854127c11 19 // Arm length when the system is turned on
ronvbree 7:a80cb6b06320 20 const float initialArmLength = L_min;
ronvbree 2:fc869e45e672 21
ronvbree 25:bdb854127c11 22 // Limit the velocities of both arms
ronvbree 18:1c9dc6caab9d 23 const float maxUpperArmVelocity = 2.1; // Rad/s
ronvbree 18:1c9dc6caab9d 24 const float maxLowerArmVelocity = 3; // Rad/s
ronvbree 12:8295c02d740f 25
ronvbree 2:fc869e45e672 26 /*
ronvbree 2:fc869e45e672 27 Constructor
ronvbree 2:fc869e45e672 28 */
ronvbree 2:fc869e45e672 29
ronvbree 2:fc869e45e672 30 Robot::Robot():
ronvbree 12:8295c02d740f 31 upperArm(initialArmLength, maxUpperArmVelocity, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
ronvbree 12:8295c02d740f 32 lowerArm(initialArmLength, maxLowerArmVelocity, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2) {
ronvbree 7:a80cb6b06320 33 killed = false;
ronvbree 2:fc869e45e672 34 }
ronvbree 2:fc869e45e672 35
ronvbree 2:fc869e45e672 36 /*
ronvbree 2:fc869e45e672 37 Methods
ronvbree 2:fc869e45e672 38 */
ronvbree 2:fc869e45e672 39
ronvbree 7:a80cb6b06320 40 // Update the arms
ronvbree 7:a80cb6b06320 41 void Robot::update() {
ronvbree 2:fc869e45e672 42 upperArm.update();
ronvbree 2:fc869e45e672 43 lowerArm.update();
ronvbree 2:fc869e45e672 44 }
ronvbree 2:fc869e45e672 45
ronvbree 12:8295c02d740f 46 // Get upper arm encoder velocity
ronvbree 12:8295c02d740f 47 float Robot::getUpperArmEncoderVelocity() {
ronvbree 12:8295c02d740f 48 return upperArm.getEncoderVelocity();
ronvbree 12:8295c02d740f 49 }
ronvbree 12:8295c02d740f 50
ronvbree 12:8295c02d740f 51 // Get lower arm encoder velocity
ronvbree 12:8295c02d740f 52 float Robot::getLowerArmEncoderVelocity() {
ronvbree 12:8295c02d740f 53 return lowerArm.getEncoderVelocity();
ronvbree 12:8295c02d740f 54 }
ronvbree 12:8295c02d740f 55
ronvbree 2:fc869e45e672 56 // Set upper arm velocity (rad/s)
ronvbree 2:fc869e45e672 57 void Robot::setUpperArmVelocity(float referenceVelocity) {
ronvbree 7:a80cb6b06320 58 if (!killed) {
ronvbree 7:a80cb6b06320 59 upperArm.setVelocity(referenceVelocity);
ronvbree 7:a80cb6b06320 60 }
ronvbree 3:1f47375270c5 61 }
ronvbree 3:1f47375270c5 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 7:a80cb6b06320 65 if (!killed) {
ronvbree 7:a80cb6b06320 66 lowerArm.setVelocity(-1 * referenceVelocity);
ronvbree 7:a80cb6b06320 67 }
ronvbree 2:fc869e45e672 68 }
ronvbree 2:fc869e45e672 69
ronvbree 2:fc869e45e672 70 // Turn off both arms
ronvbree 2:fc869e45e672 71 void Robot::kill() {
ronvbree 7:a80cb6b06320 72 killed = true;
ronvbree 2:fc869e45e672 73 upperArm.kill();
ronvbree 2:fc869e45e672 74 lowerArm.kill();
ronvbree 2:fc869e45e672 75 }
ronvbree 7:a80cb6b06320 76
ronvbree 7:a80cb6b06320 77 // Indicates if the robot has been killed
ronvbree 7:a80cb6b06320 78 bool Robot::isKilled() {
ronvbree 7:a80cb6b06320 79 return killed;
ronvbree 7:a80cb6b06320 80 }
ronvbree 7:a80cb6b06320 81
ronvbree 7:a80cb6b06320 82 // Get upper arm length
ronvbree 7:a80cb6b06320 83 float Robot::getUpperArmLength(){
ronvbree 7:a80cb6b06320 84 return upperArm.getLength();
ronvbree 7:a80cb6b06320 85 }
ronvbree 7:a80cb6b06320 86
ronvbree 7:a80cb6b06320 87 // Get lower arm length
ronvbree 7:a80cb6b06320 88 float Robot::getLowerArmLength(){
ronvbree 7:a80cb6b06320 89 return lowerArm.getLength();
ronvbree 7:a80cb6b06320 90 }