code to drive the motors to the right position

Dependencies:   HIDScope QEI mbed

Fork of BMT-K9_potmeter_fade by First Last

Point.cpp

Committer:
ewoud
Date:
2015-09-25
Revision:
6:d0f5da9962f5

File content as of revision 6:d0f5da9962f5:

// Example: data structs and functions
#include <stdio.h>
#include <math.h>

const double M_PI =3.141592653589793238463;
const float l = 10; // distance between the motors
const float armlength=15; // length of the arms from the motor
    
class Point
{
    public:
    
    float posX;
    float posY;
    float rotA;
    float rotB;

    bool fromCarthesian(float x, float y)
    {
        posX = x;
        posY = y;
        rotA = atan(y/x)*180/M_PI;
        rotB = atan(y/(l-x))*180/M_PI;
        
        // function is done, return the struct type Point:
        return true;
    }
    bool fromRotational(float a, float b)
    {
        rotA = a*M_PI/180;
        rotB = b*M_PI/180;
        posX = (tan(rotB)*l)/(tan(rotA)+tan(rotB));
        posY = tan(rotA)*posX;
        
        return true;
        
    }
    bool checkbounds()
    {
        if (rotA <= 0 or rotB <= 0){
            return 0;
        }
        if (rotA > 90 or rotB > 90){
            return 0;
        }
        if (sqrt(pow(posX,2)+pow(posY,2)) > armlength){ // too far from left arm
            return 0;
        }
        if (sqrt(pow(l-posX,2)+pow(posY,2)) > armlength){ // too far from right arm
            return 0;
        }
        return true;
    }
};