Module 9 Super Team / compute

Used for the Biorobotics Project: Calculates the position of the hands based on the shoulder rotations and vice versa.

Fork of compute by Erik Steenman

compute.cpp@10:546ff4637306, 2015-11-03 (annotated)

Committer:
AeroKev
Date:
Tue Nov 03 10:15:30 2015 +0000
Revision:
10:546ff4637306
Parent:
9:10f360732eb0 
Last Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dolcaer 0:30c99bc31f7c 1 #include <math.h>
AeroKev 6:f36e6aaf64e2 2 #include "mbed.h"
dolcaer 0:30c99bc31f7c 3 #include "compute.h"
dolcaer 0:30c99bc31f7c 4
dolcaer 0:30c99bc31f7c 5 using namespace std;
dolcaer 0:30c99bc31f7c 6
dolcaer 0:30c99bc31f7c 7 const double k = 18.77;
dolcaer 0:30c99bc31f7c 8 const double l = 26.72;
dolcaer 0:30c99bc31f7c 9 const int Ax = -7;
dolcaer 0:30c99bc31f7c 10 const int Ay = 0;
dolcaer 0:30c99bc31f7c 11 const int Bx = 7;
dolcaer 0:30c99bc31f7c 12 const int By = 0;
AeroKev 8:bfca2cc2b586 13 const double upper_arm_lim = 170*(M_PI/180);
AeroKev 7:a301b6123068 14 const double lower_arm_lim = 0;
dolcaer 0:30c99bc31f7c 15
AeroKev 7:a301b6123068 16 double calcGamma(double ApBp) {
dolcaer 0:30c99bc31f7c 17 return acos(((pow(ApBp,2))/(2*l*ApBp)));
dolcaer 0:30c99bc31f7c 18 }
dolcaer 0:30c99bc31f7c 19
AeroKev 7:a301b6123068 20 double calcOmega(double Apx, double Apy, double Bpx, double Bpy) {
dolcaer 0:30c99bc31f7c 21 return atan((Bpy-Apy)/(Bpx-Apx));
dolcaer 0:30c99bc31f7c 22 }
dolcaer 0:30c99bc31f7c 23
dolcaer 0:30c99bc31f7c 24 /*
dolcaer 0:30c99bc31f7c 25 Calculate Px in a different frame. This is the first row of the homogenous matrix (below) to calculate Px:
dolcaer 0:30c99bc31f7c 26 [cos(omega) -sin(omega) Ax]
dolcaer 0:30c99bc31f7c 27 [sin(omega) cos(omega) Ay]
dolcaer 0:30c99bc31f7c 28 [0 0 1 ]
dolcaer 0:30c99bc31f7c 29
AeroKev 7:a301b6123068 30 Input: omega (double) = the angle omega
AeroKev 7:a301b6123068 31 fPx (double) = x-coordinate of P in frame 1
AeroKev 7:a301b6123068 32 fPy (double) = y-coordinate of P in frame 1
AeroKev 7:a301b6123068 33 Apx (double) = A'x
AeroKev 7:a301b6123068 34 Output: Px in different frame (double)
dolcaer 0:30c99bc31f7c 35 */
AeroKev 7:a301b6123068 36 double homogenousX(double omega, double fPx, double fPy, double Apx) {
dolcaer 0:30c99bc31f7c 37 return (cos(omega)*fPx) - (sin(omega)*fPy) + Apx;
dolcaer 0:30c99bc31f7c 38 }
dolcaer 0:30c99bc31f7c 39
dolcaer 0:30c99bc31f7c 40 /*
dolcaer 0:30c99bc31f7c 41 Calculate Py in a different frame. This is the second row of the homogenous matrix (below) to calculate Py:
dolcaer 0:30c99bc31f7c 42 [cos(omega) -sin(omega) Ax]
dolcaer 0:30c99bc31f7c 43 [sin(omega) cos(omega) Ay]
dolcaer 0:30c99bc31f7c 44 [0 0 1 ]
dolcaer 0:30c99bc31f7c 45
AeroKev 7:a301b6123068 46 Input: omega (double) = the angle omega
AeroKev 7:a301b6123068 47 fPx (double) = x-coordinate of P in frame 1
AeroKev 7:a301b6123068 48 fPy (double) = y-coordinate of P in frame 1
AeroKev 7:a301b6123068 49 Apy (double) = A'y
AeroKev 7:a301b6123068 50 Output: Py in different frame (double)
dolcaer 0:30c99bc31f7c 51 */
AeroKev 7:a301b6123068 52 double homogenousY(double omega, double fPx, double fPy, double Apy) {
dolcaer 0:30c99bc31f7c 53 return (sin(omega)*fPx) + (cos(omega)*fPy) + Apy;
dolcaer 0:30c99bc31f7c 54 }
dolcaer 0:30c99bc31f7c 55
dolcaer 0:30c99bc31f7c 56 /*
dolcaer 0:30c99bc31f7c 57 Calculate the angle between two points A and B
dolcaer 0:30c99bc31f7c 58 */
AeroKev 7:a301b6123068 59 double calcTanDifference(double Ax, double Ay, double Bx, double By) {
dolcaer 0:30c99bc31f7c 60 return atan2((Ay-By), (Ax-Bx));
dolcaer 0:30c99bc31f7c 61 }
dolcaer 0:30c99bc31f7c 62
AeroKev 7:a301b6123068 63 double calcLengthPoints(double Ax, double Ay, double Bx, double By) {
dolcaer 0:30c99bc31f7c 64 return sqrt(pow(Ax-Bx,2) + pow(Ay-By,2));
dolcaer 0:30c99bc31f7c 65 }
dolcaer 0:30c99bc31f7c 66
AeroKev 7:a301b6123068 67 double calcCosAngle(double line) {
dolcaer 0:30c99bc31f7c 68 return acos((pow(line,2) + pow(k,2) - pow(l,2)) / (2*k*line));
dolcaer 0:30c99bc31f7c 69 }
dolcaer 0:30c99bc31f7c 70
AeroKev 7:a301b6123068 71 double calcTanDirected(double Ax, double Ay, double Bx, double By, double Ox, double Oy) {
AeroKev 7:a301b6123068 72 double rads = atan2((Ay-Oy), (Ax-Ox)) - atan2((By-Oy), (Bx-Ox));
dolcaer 0:30c99bc31f7c 73 return (rads > 0 ? rads : (2*M_PI+rads));
dolcaer 0:30c99bc31f7c 74 }
dolcaer 0:30c99bc31f7c 75
AeroKev 7:a301b6123068 76 bool checkArm(double arm) {
dolcaer 0:30c99bc31f7c 77 return (arm < upper_arm_lim && arm > lower_arm_lim);
dolcaer 0:30c99bc31f7c 78 }
dolcaer 0:30c99bc31f7c 79
AeroKev 7:a301b6123068 80 int rad2deg(double rad) {
AeroKev 5:5f75399596f3 81 return floor(rad*180);
dolcaer 0:30c99bc31f7c 82 }
dolcaer 0:30c99bc31f7c 83
AeroKev 7:a301b6123068 84 double deg2rad(double deg) {
AeroKev 5:5f75399596f3 85 return (deg/180);
dolcaer 0:30c99bc31f7c 86 }
dolcaer 0:30c99bc31f7c 87
dolcaer 0:30c99bc31f7c 88 /*
dolcaer 0:30c99bc31f7c 89 Calculate the position of Point P given two angles, alpha and beta.
dolcaer 0:30c99bc31f7c 90 Input: alpha (int) = angle alpha
dolcaer 0:30c99bc31f7c 91 beta (int) = angle beta
AeroKev 7:a301b6123068 92 double& Px, Py = needed to change Px, Py
dolcaer 0:30c99bc31f7c 93 Output: -
dolcaer 0:30c99bc31f7c 94 */
AeroKev 7:a301b6123068 95 void Angles2Point(double alpha, double beta, double& Px, double& Py) {
AeroKev 5:5f75399596f3 96 alpha = alpha*M_PI;
AeroKev 5:5f75399596f3 97 beta = beta*M_PI;
AeroKev 5:5f75399596f3 98
dolcaer 0:30c99bc31f7c 99 // Calculate A' and B' (x and y coordinate seperately)
AeroKev 7:a301b6123068 100 double Apx = k*cos(alpha)+Ax;
AeroKev 7:a301b6123068 101 double Apy = k*sin(alpha)+Ay;
AeroKev 7:a301b6123068 102 double Bpx = k*cos(beta)+Bx;
AeroKev 7:a301b6123068 103 double Bpy = k*sin(beta)+By;
dolcaer 0:30c99bc31f7c 104
AeroKev 7:a301b6123068 105 double ApBp = calcLengthPoints(Apx, Apy, Bpx, Bpy);
dolcaer 0:30c99bc31f7c 106
dolcaer 0:30c99bc31f7c 107 // Calculate gamma
AeroKev 7:a301b6123068 108 double gamma = calcGamma(ApBp);
dolcaer 0:30c99bc31f7c 109
dolcaer 0:30c99bc31f7c 110 // Calculate Px and Py in frame 1
AeroKev 7:a301b6123068 111 double fPx = l*cos(gamma);
AeroKev 7:a301b6123068 112 double fPy = l*sin(gamma);
dolcaer 0:30c99bc31f7c 113
dolcaer 0:30c99bc31f7c 114 // Calculate omega, angle between frame 0 and frame 1
AeroKev 7:a301b6123068 115 double omega = calcOmega(Apx, Apy, Bpx, Bpy);
dolcaer 0:30c99bc31f7c 116
dolcaer 0:30c99bc31f7c 117 // Calculate Px and Py in frame 0 and update it
dolcaer 0:30c99bc31f7c 118 Px = homogenousX(omega, fPx, fPy, Apx);
dolcaer 0:30c99bc31f7c 119 Py = homogenousY(omega, fPx, fPy, Apy);
dolcaer 0:30c99bc31f7c 120
dolcaer 0:30c99bc31f7c 121 // Check physical constraints
AeroKev 7:a301b6123068 122 double armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy);
AeroKev 7:a301b6123068 123 double armB = calcTanDirected(Bx, By, Px, Py, Bpx, Bpy);
dolcaer 0:30c99bc31f7c 124 if(!(checkArm(armA) && checkArm(armB))) {
dolcaer 0:30c99bc31f7c 125 return;
dolcaer 0:30c99bc31f7c 126 }
dolcaer 0:30c99bc31f7c 127
dolcaer 0:30c99bc31f7c 128 }
dolcaer 0:30c99bc31f7c 129
dolcaer 0:30c99bc31f7c 130
dolcaer 0:30c99bc31f7c 131 void Point2Angles(double Px, double Py, double& a, double& b) {
dolcaer 0:30c99bc31f7c 132 // Calculate gamma of the arm A and arm B
AeroKev 7:a301b6123068 133 double gamma_a = calcTanDifference(Px, Py, Ax, Ay);
AeroKev 7:a301b6123068 134 double gamma_b = calcTanDifference(Px, Py, Bx, By)*(-1);
dolcaer 0:30c99bc31f7c 135
dolcaer 0:30c99bc31f7c 136 // Calculate |AP| and |BP|
AeroKev 7:a301b6123068 137 double AP = calcLengthPoints(Px, Py, Ax, Ay);
AeroKev 7:a301b6123068 138 double BP = calcLengthPoints(Px, Py, Bx, By);
dolcaer 0:30c99bc31f7c 139
dolcaer 0:30c99bc31f7c 140
dolcaer 0:30c99bc31f7c 141 // Calculate omega of the arm A and arm B
AeroKev 7:a301b6123068 142 double omega_a = calcCosAngle(AP);
AeroKev 7:a301b6123068 143 double omega_b = calcCosAngle(BP);
dolcaer 0:30c99bc31f7c 144
dolcaer 0:30c99bc31f7c 145 // Calculate A'(A'x, A'y) and B'(A'x, A'y) in the new frame
AeroKev 7:a301b6123068 146 double Apx2 = k*cos(omega_a);
AeroKev 7:a301b6123068 147 double Apy2 = k*sin(omega_a);
AeroKev 7:a301b6123068 148 double Bpx2 = k*cos(omega_b);
AeroKev 7:a301b6123068 149 double Bpy2 = k*sin(omega_b);
dolcaer 0:30c99bc31f7c 150
dolcaer 0:30c99bc31f7c 151 // Translate and rotate A' and B' from frame 2 to frame 1 using gamma.
AeroKev 7:a301b6123068 152 double Apx = homogenousX(gamma_a, Apx2, Apy2, Ax);
AeroKev 7:a301b6123068 153 double Apy = homogenousY(gamma_a, Apx2, Apy2, Ay);
AeroKev 7:a301b6123068 154 double Bpx = homogenousX(gamma_b, Bpx2, Bpy2, Bx);
AeroKev 7:a301b6123068 155 double Bpy = homogenousY(gamma_b, Bpx2, Bpy2, By);
dolcaer 0:30c99bc31f7c 156
dolcaer 0:30c99bc31f7c 157
dolcaer 0:30c99bc31f7c 158 // Calculate alpha and beta
dolcaer 0:30c99bc31f7c 159 a = calcTanDifference(Apx, Apy, Ax, Ay);
dolcaer 0:30c99bc31f7c 160 b = calcTanDifference(Bpx, Bpy, Bx, By)*(-1);
dolcaer 0:30c99bc31f7c 161
dolcaer 0:30c99bc31f7c 162 // If alpha < 0, make alpha positive
dolcaer 0:30c99bc31f7c 163 if(a<0) {
AeroKev 7:a301b6123068 164 double alpha = (2*M_PI)+a;
dolcaer 0:30c99bc31f7c 165 a = alpha;
dolcaer 0:30c99bc31f7c 166 }
dolcaer 0:30c99bc31f7c 167
dolcaer 0:30c99bc31f7c 168 // Check physical constraints
AeroKev 7:a301b6123068 169 double armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy)/M_PI;
AeroKev 7:a301b6123068 170 double armB = calcTanDirected(Bx, By, Px, Py, Bpx, Bpy*(-1))/M_PI;
AeroKev 6:f36e6aaf64e2 171 if(armA*180>170 || armB*180>170) {
dolcaer 0:30c99bc31f7c 172 a = 100;
dolcaer 0:30c99bc31f7c 173 b = 100;
dolcaer 0:30c99bc31f7c 174 }
AeroKev 5:5f75399596f3 175
AeroKev 5:5f75399596f3 176 a = a/M_PI;
AeroKev 5:5f75399596f3 177 b = b/M_PI;
dolcaer 0:30c99bc31f7c 178 }
dolcaer 0:30c99bc31f7c 179
AeroKev 9:10f360732eb0 180 int compute_max(char dir, int pos) {
dolcaer 0:30c99bc31f7c 181 double a,b;
AeroKev 7:a301b6123068 182 double high = (dir=='X') ? 35 : 44;
AeroKev 7:a301b6123068 183 double cur = (high/2);
AeroKev 7:a301b6123068 184 double low = 0;
dolcaer 0:30c99bc31f7c 185 while(abs(high-low)>0.00001) {
dolcaer 0:30c99bc31f7c 186 if(dir=='X') Point2Angles(cur, pos, a, b);
dolcaer 0:30c99bc31f7c 187 else Point2Angles(pos, cur, a, b);
AeroKev 6:f36e6aaf64e2 188 if(a < 31 && b < 31) {
dolcaer 0:30c99bc31f7c 189 low = cur;
dolcaer 0:30c99bc31f7c 190 cur = (high+low)/2;
dolcaer 0:30c99bc31f7c 191 if(cur>high) cur = high;
dolcaer 0:30c99bc31f7c 192 } else {
dolcaer 0:30c99bc31f7c 193 high = cur;
dolcaer 0:30c99bc31f7c 194 cur = (high+low)/2;
dolcaer 0:30c99bc31f7c 195 if(cur<low) cur = low;
dolcaer 0:30c99bc31f7c 196 }
dolcaer 0:30c99bc31f7c 197 }
AeroKev 6:f36e6aaf64e2 198 return (dir == 'X') ? low : (low-1);
dolcaer 0:30c99bc31f7c 199 }