Project Paint / Mbed 2 deprecated arm_control

Dependencies:   mbed QEI biquadFilter

geometry.cpp

Committer:
ronvbree
Date:
2016-11-02
Revision:
2:fc869e45e672
Parent:
0:494acf21d3bc
Child:
7:a80cb6b06320

File content as of revision 2:fc869e45e672:

#include "math.h" 
#include "geometry.h"  

/*
    Methods
*/

// Sets the required arm lengths in the 'upper' and 'lower' parameters
void getArmLengthsForRollerPosition(float x, float y, float &upper, float &lower) {
    lower = sqrt(pow(y-h,2)+pow(x,2));
    upper = sqrt(pow(lower,2)+pow(d,2)-2*d*sqrt(pow(lower,2)-pow(x,2)));
}

// Sets the roller position corresponding to the current arm lengths in the x and y parameters
void getRollerPositionForArmLengths(float upper, float lower, float &x, float &y) {
    y=(pow(lower,2)-pow(upper,2)+2*h*d+pow(d,2))/(2*d);
    x=sqrt(pow(upper,2)-pow(y-h,2)); //TODO
}

/*
    Getters
*/

float getMaximalArmLength(){
    return L_max;
}

float getMinimalArmLength(){
    return L_min;
}

float getReach(){
    return reach;
}

float getMaximalRollerHeight(){
    return y_max;
}

float getMinimalRollerHeight(){
    return y_min;
}