Peter Knoben / AnglePosition2

angledfa

Dependents:   kinematics_controlv4 kinematics_control_copyfds Robot_control ShowIt

Fork of AnglePosition by Peter Knoben

AnglePosition.cpp@1:5c789825341d, 2017-10-31 (annotated)

Committer:
peterknoben
Date:
Tue Oct 31 14:34:23 2017 +0000
Revision:
1:5c789825341d
Parent:
0:d7e19af20f93
Child:
2:20afba507e9e
jhkdsg

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 1:5c789825341d 11 float AnglePosition::gettargetpositionpot(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 1:5c789825341d 16 float AnglePosition::gettargetposition(float input, int max_range){
peterknoben 1:5c789825341d 17 float target;
peterknoben 1:5c789825341d 18 if(input<=max_range && input>=0){
peterknoben 1:5c789825341d 19 target = input;
peterknoben 1:5c789825341d 20 }
peterknoben 1:5c789825341d 21 else{
peterknoben 1:5c789825341d 22 target = target;
peterknoben 1:5c789825341d 23 }
peterknoben 1:5c789825341d 24 return target;
peterknoben 1:5c789825341d 25 }
peterknoben 1:5c789825341d 26
peterknoben 0:d7e19af20f93 27
peterknoben 0:d7e19af20f93 28 float AnglePosition::getreferenceposition(float target, float offset) {
peterknoben 0:d7e19af20f93 29 float referenceposition = target + offset;
peterknoben 0:d7e19af20f93 30 return referenceposition;
peterknoben 0:d7e19af20f93 31 }
peterknoben 0:d7e19af20f93 32
peterknoben 0:d7e19af20f93 33
peterknoben 0:d7e19af20f93 34 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 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 alpha = alpha_ + alpha_offset;
peterknoben 0:d7e19af20f93 41 return alpha;
peterknoben 0:d7e19af20f93 42 }
peterknoben 0:d7e19af20f93 43
peterknoben 0:d7e19af20f93 44
peterknoben 0:d7e19af20f93 45 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 46 float x_target = gettargetposition(inputx, max_rangex);
peterknoben 0:d7e19af20f93 47 float y_target = gettargetposition(inputy, max_rangey);
peterknoben 0:d7e19af20f93 48 float x = getreferenceposition(x_target, x_offset);
peterknoben 0:d7e19af20f93 49 float y = getreferenceposition(y_target, y_offset);
peterknoben 0:d7e19af20f93 50 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 51 float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
peterknoben 0:d7e19af20f93 52 return beta;
peterknoben 0:d7e19af20f93 53 }
peterknoben 0:d7e19af20f93 54