Craig Evans / Leg

Library to control a 3DOF robotic leg. Specify an [x,y,z] co-ordinate and the library will solve the inverse kinematic equations to calculate the required servo pulse widths to position the foot at the target location.

Dependencies:   Utils

Leg.h@6:a91d18a26ba3, 2015-05-25 (annotated)

Committer:
eencae
Date:
Mon May 25 15:04:57 2015 +0000
Revision:
6:a91d18a26ba3
Parent:
5:1190596b8842 
Added default robotic leg dimensions.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:74b4e50e0d15 1 /**
eencae 0:74b4e50e0d15 2 @file Leg.h
eencae 0:74b4e50e0d15 3
eencae 0:74b4e50e0d15 4 @brief Header file containing member functions and variables
eencae 0:74b4e50e0d15 5
eencae 0:74b4e50e0d15 6 */
eencae 0:74b4e50e0d15 7
eencae 0:74b4e50e0d15 8 #ifndef LEG_H
eencae 0:74b4e50e0d15 9 #define LEG_H
eencae 0:74b4e50e0d15 10
eencae 0:74b4e50e0d15 11 #include "mbed.h"
eencae 4:e95f55af0d74 12 #include "Utils.h"
eencae 0:74b4e50e0d15 13
eencae 0:74b4e50e0d15 14 /**
eencae 0:74b4e50e0d15 15 @brief mbed library for controlling a 3DOF robotic leg such as though found on the Lynxmotion MAH3-R Hexapod.
eencae 0:74b4e50e0d15 16 @brief The library solves the inverse kinematic equations of the leg to position the foot at a
eencae 0:74b4e50e0d15 17 @brief specified [X,Y,Z] cordinate.
eencae 0:74b4e50e0d15 18
eencae 0:74b4e50e0d15 19 @see http://www.lynxmotion.com/c-157-mah3-r.aspx
eencae 0:74b4e50e0d15 20
eencae 0:74b4e50e0d15 21 @brief Revision 0.1
eencae 0:74b4e50e0d15 22
eencae 0:74b4e50e0d15 23 @author Craig A. Evans
eencae 0:74b4e50e0d15 24 @date May 2015
eencae 0:74b4e50e0d15 25 *
eencae 0:74b4e50e0d15 26 * Example:
eencae 0:74b4e50e0d15 27 * @code
eencae 0:74b4e50e0d15 28
eencae 0:74b4e50e0d15 29 #include "mbed.h"
eencae 0:74b4e50e0d15 30 #include "Leg.h"
eencae 0:74b4e50e0d15 31
eencae 0:74b4e50e0d15 32 Serial serial(USBTX,USBRX);
eencae 0:74b4e50e0d15 33 Leg leg(p21,p22,p23);
eencae 0:74b4e50e0d15 34
eencae 0:74b4e50e0d15 35 int main()
eencae 0:74b4e50e0d15 36 {
eencae 0:74b4e50e0d15 37 serial.baud(115200);
eencae 0:74b4e50e0d15 38
eencae 0:74b4e50e0d15 39 leg.set_link_lengths(28.0,58.0,125.0); // set leg dimensions
eencae 0:74b4e50e0d15 40
eencae 0:74b4e50e0d15 41 Timer t; // timer for calculation (~80 us on LPC1768)
eencae 0:74b4e50e0d15 42 t.start();
eencae 5:1190596b8842 43 vector_t angles = leg.solve_inverse_kinematics(80.0,0.0,-100.0); // solve IK equations
eencae 0:74b4e50e0d15 44 t.stop();
eencae 0:74b4e50e0d15 45
eencae 0:74b4e50e0d15 46 serial.printf("\nSolution time = %f us\n",1e6*t.read());
eencae 0:74b4e50e0d15 47
eencae 5:1190596b8842 48 leg.move(angles); // update servo angles
eencae 0:74b4e50e0d15 49
eencae 0:74b4e50e0d15 50 while(1) {
eencae 0:74b4e50e0d15 51
eencae 0:74b4e50e0d15 52 }
eencae 0:74b4e50e0d15 53 }
eencae 0:74b4e50e0d15 54
eencae 0:74b4e50e0d15 55 * @endcode
eencae 0:74b4e50e0d15 56 */
eencae 0:74b4e50e0d15 57
eencae 0:74b4e50e0d15 58 class Leg
eencae 0:74b4e50e0d15 59 {
eencae 0:74b4e50e0d15 60
eencae 0:74b4e50e0d15 61 public:
eencae 0:74b4e50e0d15 62 /** Create a Leg object connected to the specified pins. MUST be PWM-enabled pins.
eencae 0:74b4e50e0d15 63 *
eencae 0:74b4e50e0d15 64 * @param coxa_pin Signal line for the coxa servo
eencae 0:74b4e50e0d15 65 * @param femur_pin Signal line for the femur servo
eencae 0:74b4e50e0d15 66 * @param tibia_pin Signal line for the tibia servo
eencae 0:74b4e50e0d15 67 *
eencae 0:74b4e50e0d15 68 */
eencae 0:74b4e50e0d15 69 Leg(PinName coxa_pin, PinName femur_pin, PinName tibia_pin);
eencae 0:74b4e50e0d15 70
eencae 0:74b4e50e0d15 71 /** Solve inverse kinematic equations to get the required joint angles
eencae 0:74b4e50e0d15 72 * The x-axis is defined as the common normal of the joints.
eencae 0:74b4e50e0d15 73 * The coxa joint rotates about the z-axis.
eencae 0:74b4e50e0d15 74 * The y-axis is defined by the right-hand rule.
eencae 0:74b4e50e0d15 75 * @param x Distance away from body
eencae 0:74b4e50e0d15 76 * @param y Distance in front/behind the hip
eencae 0:74b4e50e0d15 77 * @param z Height of foot
eencae 5:1190596b8842 78 * @returns a vector containing the joint angles [coxa,femur,tibia]
eencae 0:74b4e50e0d15 79 */
eencae 5:1190596b8842 80 vector_t solve_inverse_kinematics(float x, float y, float z);
eencae 0:74b4e50e0d15 81
eencae 0:74b4e50e0d15 82 /** Set link lengths (in mm).
eencae 0:74b4e50e0d15 83 * @param coxa_length Length of coxa
eencae 0:74b4e50e0d15 84 * @param femur_length Length of femur
eencae 0:74b4e50e0d15 85 * @param tibia_length Length of tibia
eencae 0:74b4e50e0d15 86 */
eencae 0:74b4e50e0d15 87 void set_link_lengths(float coxa_length, float femur_length, float tibia_length);
eencae 0:74b4e50e0d15 88
eencae 0:74b4e50e0d15 89 /** Move servos to calculated angles
eencae 5:1190596b8842 90 * @param a vector containing the joint angles [coxa,femur,tibia]
eencae 0:74b4e50e0d15 91 */
eencae 5:1190596b8842 92 void move(vector_t angles);
eencae 0:74b4e50e0d15 93
eencae 0:74b4e50e0d15 94 private:
eencae 0:74b4e50e0d15 95
eencae 0:74b4e50e0d15 96 public:
eencae 0:74b4e50e0d15 97
eencae 0:74b4e50e0d15 98
eencae 0:74b4e50e0d15 99
eencae 0:74b4e50e0d15 100 private: // private variables
eencae 0:74b4e50e0d15 101 // pins for servos
eencae 0:74b4e50e0d15 102 PwmOut* coxa_servo;
eencae 0:74b4e50e0d15 103 PwmOut* femur_servo;
eencae 0:74b4e50e0d15 104 PwmOut* tibia_servo;
eencae 5:1190596b8842 105 //Serial* serial; // for comms
eencae 0:74b4e50e0d15 106
eencae 0:74b4e50e0d15 107 // link lengths
eencae 0:74b4e50e0d15 108 float coxa_length_;
eencae 0:74b4e50e0d15 109 float femur_length_;
eencae 0:74b4e50e0d15 110 float tibia_length_;
eencae 0:74b4e50e0d15 111
eencae 0:74b4e50e0d15 112 };
eencae 0:74b4e50e0d15 113
eencae 0:74b4e50e0d15 114 #endif