Peter Knoben / AnglePosition2

angledfa

Dependents:   kinematics_controlv4 kinematics_control_copyfds Robot_control ShowIt

Fork of AnglePosition by Peter Knoben

AnglePosition.cpp@0:d7e19af20f93, 2017-10-26 (annotated)

Committer:
peterknoben
Date:
Thu Oct 26 10:53:29 2017 +0000
Revision:
0:d7e19af20f93
Child:
1:5c789825341d
Inclusief header files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 0:d7e19af20f93 1 #include "AnglePosition.h"
peterknoben 0:d7e19af20f93 2 #include "mbed.h"
peterknoben 0:d7e19af20f93 3
peterknoben 0:d7e19af20f93 4 const double PI = 3.14159265358979323846;
peterknoben 0:d7e19af20f93 5
peterknoben 0:d7e19af20f93 6 AnglePosition::AnglePosition(void)
peterknoben 0:d7e19af20f93 7 {
peterknoben 0:d7e19af20f93 8
peterknoben 0:d7e19af20f93 9 }
peterknoben 0:d7e19af20f93 10
peterknoben 0:d7e19af20f93 11 float AnglePosition::gettargetposition(double input, int max_range){
peterknoben 0:d7e19af20f93 12 float target = input * max_range;
peterknoben 0:d7e19af20f93 13 return target;
peterknoben 0:d7e19af20f93 14 }
peterknoben 0:d7e19af20f93 15
peterknoben 0:d7e19af20f93 16
peterknoben 0:d7e19af20f93 17 float AnglePosition::getreferenceposition(float target, float offset) {
peterknoben 0:d7e19af20f93 18 float referenceposition = target + offset;
peterknoben 0:d7e19af20f93 19 return referenceposition;
peterknoben 0:d7e19af20f93 20 }
peterknoben 0:d7e19af20f93 21
peterknoben 0:d7e19af20f93 22
peterknoben 0:d7e19af20f93 23 float AnglePosition::getalpha(float max_rangex, float max_rangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2, double inputx, double inputy) {
peterknoben 0:d7e19af20f93 24 float x_target = gettargetposition(inputx, max_rangex);
peterknoben 0:d7e19af20f93 25 float y_target = gettargetposition(inputy, max_rangey);
peterknoben 0:d7e19af20f93 26 float x = getreferenceposition(x_target, x_offset);
peterknoben 0:d7e19af20f93 27 float y = getreferenceposition(y_target, y_offset);
peterknoben 0:d7e19af20f93 28 float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
peterknoben 0:d7e19af20f93 29 float alpha = alpha_ + alpha_offset;
peterknoben 0:d7e19af20f93 30 return alpha;
peterknoben 0:d7e19af20f93 31 }
peterknoben 0:d7e19af20f93 32
peterknoben 0:d7e19af20f93 33
peterknoben 0:d7e19af20f93 34 float AnglePosition::getbeta(float max_rangex, float max_rangey, float x_offset, float y_offset, float beta_offset, float L1, float L2, double inputx, double inputy) {
peterknoben 0:d7e19af20f93 35 float x_target = gettargetposition(inputx, max_rangex);
peterknoben 0:d7e19af20f93 36 float y_target = gettargetposition(inputy, max_rangey);
peterknoben 0:d7e19af20f93 37 float x = getreferenceposition(x_target, x_offset);
peterknoben 0:d7e19af20f93 38 float y = getreferenceposition(y_target, y_offset);
peterknoben 0:d7e19af20f93 39 float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
peterknoben 0:d7e19af20f93 40 float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
peterknoben 0:d7e19af20f93 41 return beta;
peterknoben 0:d7e19af20f93 42 }
peterknoben 0:d7e19af20f93 43