Austin Mitchell / Mbed 2 deprecated RobotFightingControl

Sword fighting robots WIP

Dependencies:   mbed PinDetect

ik.cpp@0:e8eecd4b9a3d, 2018-12-06 (annotated)

Committer:
amitchell41
Date:
Thu Dec 06 01:30:51 2018 +0000
Revision:
0:e8eecd4b9a3d
Swing batta batta swing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amitchell41 0:e8eecd4b9a3d 1 /* Inverse kinetics, Nick Moriarty May 2014
amitchell41 0:e8eecd4b9a3d 2 This code is provided under the terms of the MIT license.
amitchell41 0:e8eecd4b9a3d 3 The MIT License (MIT)
amitchell41 0:e8eecd4b9a3d 4 Copyright (c) 2014 Nick Moriarty
amitchell41 0:e8eecd4b9a3d 5 Permission is hereby granted, free of charge, to any person obtaining a copy
amitchell41 0:e8eecd4b9a3d 6 of this software and associated documentation files (the "Software"), to deal
amitchell41 0:e8eecd4b9a3d 7 in the Software without restriction, including without limitation the rights
amitchell41 0:e8eecd4b9a3d 8 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
amitchell41 0:e8eecd4b9a3d 9 copies of the Software, and to permit persons to whom the Software is
amitchell41 0:e8eecd4b9a3d 10 furnished to do so, subject to the following conditions:
amitchell41 0:e8eecd4b9a3d 11 The above copyright notice and this permission notice shall be included in
amitchell41 0:e8eecd4b9a3d 12 all copies or substantial portions of the Software.
amitchell41 0:e8eecd4b9a3d 13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
amitchell41 0:e8eecd4b9a3d 14 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
amitchell41 0:e8eecd4b9a3d 15 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
amitchell41 0:e8eecd4b9a3d 16 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
amitchell41 0:e8eecd4b9a3d 17 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
amitchell41 0:e8eecd4b9a3d 18 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
amitchell41 0:e8eecd4b9a3d 19 SOFTWARE.
amitchell41 0:e8eecd4b9a3d 20 */
amitchell41 0:e8eecd4b9a3d 21 #include "math.h"
amitchell41 0:e8eecd4b9a3d 22 #include "ik.h"
amitchell41 0:e8eecd4b9a3d 23 #include "config.h"
amitchell41 0:e8eecd4b9a3d 24
amitchell41 0:e8eecd4b9a3d 25 const float PI=3.14159265359;
amitchell41 0:e8eecd4b9a3d 26
amitchell41 0:e8eecd4b9a3d 27 // Get polar coords from cartesian ones
amitchell41 0:e8eecd4b9a3d 28 void cart2polar(float a, float b, float& r, float& theta)
amitchell41 0:e8eecd4b9a3d 29 {
amitchell41 0:e8eecd4b9a3d 30 // Determine magnitude of cartesian coords
amitchell41 0:e8eecd4b9a3d 31 r = sqrt(a*a + b*b);
amitchell41 0:e8eecd4b9a3d 32
amitchell41 0:e8eecd4b9a3d 33 // Don't try to calculate zero-magnitude vectors' angles
amitchell41 0:e8eecd4b9a3d 34 if(r == 0) return;
amitchell41 0:e8eecd4b9a3d 35
amitchell41 0:e8eecd4b9a3d 36 float c = a / r;
amitchell41 0:e8eecd4b9a3d 37 float s = b / r;
amitchell41 0:e8eecd4b9a3d 38
amitchell41 0:e8eecd4b9a3d 39 // Safety!
amitchell41 0:e8eecd4b9a3d 40 if(s > 1) s = 1;
amitchell41 0:e8eecd4b9a3d 41 if(c > 1) c = 1;
amitchell41 0:e8eecd4b9a3d 42 if(s < -1) s = -1;
amitchell41 0:e8eecd4b9a3d 43 if(c < -1) c = -1;
amitchell41 0:e8eecd4b9a3d 44
amitchell41 0:e8eecd4b9a3d 45 // Calculate angle in 0..PI
amitchell41 0:e8eecd4b9a3d 46 theta = acos(c);
amitchell41 0:e8eecd4b9a3d 47
amitchell41 0:e8eecd4b9a3d 48 // Convert to full range
amitchell41 0:e8eecd4b9a3d 49 if(s < 0) theta *= -1;
amitchell41 0:e8eecd4b9a3d 50 }
amitchell41 0:e8eecd4b9a3d 51
amitchell41 0:e8eecd4b9a3d 52 // Get angle from a triangle using cosine rule
amitchell41 0:e8eecd4b9a3d 53 bool cosangle(float opp, float adj1, float adj2, float& theta)
amitchell41 0:e8eecd4b9a3d 54 {
amitchell41 0:e8eecd4b9a3d 55 // Cosine rule:
amitchell41 0:e8eecd4b9a3d 56 // C^2 = A^2 + B^2 - 2*A*B*cos(angle_AB)
amitchell41 0:e8eecd4b9a3d 57 // cos(angle_AB) = (A^2 + B^2 - C^2)/(2*A*B)
amitchell41 0:e8eecd4b9a3d 58 // C is opposite
amitchell41 0:e8eecd4b9a3d 59 // A, B are adjacent
amitchell41 0:e8eecd4b9a3d 60 float den = 2*adj1*adj2;
amitchell41 0:e8eecd4b9a3d 61
amitchell41 0:e8eecd4b9a3d 62 if(den==0) return false;
amitchell41 0:e8eecd4b9a3d 63 float c = (adj1*adj1 + adj2*adj2 - opp*opp)/den;
amitchell41 0:e8eecd4b9a3d 64
amitchell41 0:e8eecd4b9a3d 65 if(c>1 || c<-1) return false;
amitchell41 0:e8eecd4b9a3d 66
amitchell41 0:e8eecd4b9a3d 67 theta = acos(c);
amitchell41 0:e8eecd4b9a3d 68
amitchell41 0:e8eecd4b9a3d 69 return true;
amitchell41 0:e8eecd4b9a3d 70 }
amitchell41 0:e8eecd4b9a3d 71
amitchell41 0:e8eecd4b9a3d 72 // Solve angles!
amitchell41 0:e8eecd4b9a3d 73 bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
amitchell41 0:e8eecd4b9a3d 74 {
amitchell41 0:e8eecd4b9a3d 75 // Solve top-down view
amitchell41 0:e8eecd4b9a3d 76 float r, th0;
amitchell41 0:e8eecd4b9a3d 77 cart2polar(y, x, r, th0);
amitchell41 0:e8eecd4b9a3d 78
amitchell41 0:e8eecd4b9a3d 79 // Account for the wrist length!
amitchell41 0:e8eecd4b9a3d 80 r -= L3;
amitchell41 0:e8eecd4b9a3d 81
amitchell41 0:e8eecd4b9a3d 82 // In arm plane, convert to polar
amitchell41 0:e8eecd4b9a3d 83 float ang_P, R;
amitchell41 0:e8eecd4b9a3d 84 cart2polar(r, z, R, ang_P);
amitchell41 0:e8eecd4b9a3d 85
amitchell41 0:e8eecd4b9a3d 86 // Solve arm inner angles as required
amitchell41 0:e8eecd4b9a3d 87 float B, C;
amitchell41 0:e8eecd4b9a3d 88 if(!cosangle(L2, L1, R, B)) return false;
amitchell41 0:e8eecd4b9a3d 89 if(!cosangle(R, L1, L2, C)) return false;
amitchell41 0:e8eecd4b9a3d 90
amitchell41 0:e8eecd4b9a3d 91 // Solve for servo angles from horizontal
amitchell41 0:e8eecd4b9a3d 92 a0 = th0;
amitchell41 0:e8eecd4b9a3d 93 a1 = ang_P + B;
amitchell41 0:e8eecd4b9a3d 94 a2 = C + a1 - PI;
amitchell41 0:e8eecd4b9a3d 95
amitchell41 0:e8eecd4b9a3d 96 return true;
amitchell41 0:e8eecd4b9a3d 97 }