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

arm.cpp@18:1c9dc6caab9d, 2016-11-03 (annotated)

Committer:
ronvbree
Date:
Thu Nov 03 16:29:31 2016 +0000
Revision:
18:1c9dc6caab9d
Parent:
12:8295c02d740f
Child:
29:1c5254b27851
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ronvbree 0:494acf21d3bc 1 #include "mbed.h"
ronvbree 0:494acf21d3bc 2 #include "arm.h"
ronvbree 0:494acf21d3bc 3
ronvbree 0:494acf21d3bc 4 /*
ronvbree 0:494acf21d3bc 5 Arm Constructor
ronvbree 0:494acf21d3bc 6 */
ronvbree 0:494acf21d3bc 7
ronvbree 0:494acf21d3bc 8 // Create an arm
ronvbree 12:8295c02d740f 9 Arm::Arm(float initialLength, float maxVelocity, PinName motorPWM, PinName motorDir, PinName encoderPin1, PinName encoderPin2)
ronvbree 12:8295c02d740f 10 : motorControl(motorPWM)
ronvbree 12:8295c02d740f 11 , motorDirection(motorDir)
ronvbree 12:8295c02d740f 12 , qei(encoderPin1, encoderPin2, NC, pulsesPerRevolution)
ronvbree 12:8295c02d740f 13 {
ronvbree 0:494acf21d3bc 14 length = initialLength;
ronvbree 12:8295c02d740f 15 maxAllowedVelocity = maxVelocity;
ronvbree 0:494acf21d3bc 16 velocity = 0;
ronvbree 2:fc869e45e672 17 encoderVelocity = 0;
ronvbree 0:494acf21d3bc 18 motorDirection = clockwise;
ronvbree 0:494acf21d3bc 19 }
ronvbree 0:494acf21d3bc 20
ronvbree 0:494acf21d3bc 21 /*
ronvbree 0:494acf21d3bc 22 Methods
ronvbree 0:494acf21d3bc 23 */
ronvbree 0:494acf21d3bc 24
ronvbree 0:494acf21d3bc 25 // Update the arm's instance variables
ronvbree 2:fc869e45e672 26 void Arm::update() {
ronvbree 7:a80cb6b06320 27 updateEncoderVelocity();
ronvbree 7:a80cb6b06320 28 length += tick * encoderVelocity * gearRadius; // Update length from angular velocity
ronvbree 2:fc869e45e672 29 }
ronvbree 2:fc869e45e672 30
ronvbree 2:fc869e45e672 31 // Read pulses since previous measurement to estimate angular velocity
ronvbree 2:fc869e45e672 32 void Arm::updateEncoderVelocity() {
ronvbree 2:fc869e45e672 33 encoderVelocity = ((qei.getPulses()/outputShaftResolution))/tick;
ronvbree 2:fc869e45e672 34 qei.reset();
ronvbree 0:494acf21d3bc 35 }
ronvbree 0:494acf21d3bc 36
ronvbree 0:494acf21d3bc 37 // Set a new reference velocity
ronvbree 0:494acf21d3bc 38 void Arm::setVelocity(float referenceVelocity) {
ronvbree 7:a80cb6b06320 39 // Counterclockwise rotation
ronvbree 2:fc869e45e672 40 if (referenceVelocity < 0) {
ronvbree 0:494acf21d3bc 41 motorDirection = counterClockwise;
ronvbree 18:1c9dc6caab9d 42 velocity = (referenceVelocity < (-1 * maxAllowedVelocity))? (-1 * maxAllowedVelocity):referenceVelocity;
ronvbree 7:a80cb6b06320 43 // Clockwise rotation
ronvbree 0:494acf21d3bc 44 } else {
ronvbree 0:494acf21d3bc 45 motorDirection = clockwise;
ronvbree 12:8295c02d740f 46 velocity = (referenceVelocity > maxAllowedVelocity)? maxAllowedVelocity:referenceVelocity;
ronvbree 0:494acf21d3bc 47 }
ronvbree 7:a80cb6b06320 48 // Output to motor
ronvbree 7:a80cb6b06320 49 setMotor(velocity/motorGain);
ronvbree 0:494acf21d3bc 50 }
ronvbree 0:494acf21d3bc 51
ronvbree 2:fc869e45e672 52 // Set motor value (ranges from 0 to 1)
ronvbree 0:494acf21d3bc 53 void Arm::setMotor(float motorValue) {
ronvbree 7:a80cb6b06320 54 motorControl.write((fabs(motorValue) > 1)? 1 : fabs(motorValue));
ronvbree 2:fc869e45e672 55 }
ronvbree 2:fc869e45e672 56
ronvbree 2:fc869e45e672 57 // Stop moving
ronvbree 2:fc869e45e672 58 void Arm::kill() {
ronvbree 2:fc869e45e672 59 setVelocity(0);
ronvbree 0:494acf21d3bc 60 }
ronvbree 0:494acf21d3bc 61
ronvbree 0:494acf21d3bc 62 /*
ronvbree 7:a80cb6b06320 63 Getters
ronvbree 0:494acf21d3bc 64 */
ronvbree 0:494acf21d3bc 65
ronvbree 0:494acf21d3bc 66 float Arm::getLength() {
ronvbree 0:494acf21d3bc 67 return length;
ronvbree 0:494acf21d3bc 68 }
ronvbree 0:494acf21d3bc 69
ronvbree 2:fc869e45e672 70 float Arm::getEncoderVelocity() {
ronvbree 2:fc869e45e672 71 return encoderVelocity;
ronvbree 2:fc869e45e672 72 }
ronvbree 2:fc869e45e672 73
ronvbree 0:494acf21d3bc 74 float Arm::getVelocity() {
ronvbree 0:494acf21d3bc 75 return velocity;
ronvbree 0:494acf21d3bc 76 }