marijn peters / Mbed 2 deprecated test_HIDScope

testen of de juiste hoeken gevonden worden

Dependencies:   HIDScope mbed

Fork of get_angles by marijn peters

main.cpp@2:e2af01393e16, 2015-10-16 (annotated)

Committer:
marijnstudent
Date:
Fri Oct 16 08:56:32 2015 +0000
Revision:
2:e2af01393e16
Parent:
1:a93b870d3cf8 
zat een foutje in de formule

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marijnstudent 0:121acb083576 1 #include "mbed.h"
marijnstudent 0:121acb083576 2 #include "math.h"
marijnstudent 1:a93b870d3cf8 3 #include "HIDScope.h"
marijnstudent 0:121acb083576 4
marijnstudent 1:a93b870d3cf8 5 HIDScope scope(4);
marijnstudent 1:a93b870d3cf8 6 AnalogIn potright(A0);
marijnstudent 1:a93b870d3cf8 7 AnalogIn potleft(A1);
marijnstudent 0:121acb083576 8 // x & y zijn inputs L is lengte van de arm r is de vector naar de end effector
marijnstudent 1:a93b870d3cf8 9
marijnstudent 0:121acb083576 10 const double L = 36;
marijnstudent 0:121acb083576 11 const double pi = 3.1415926535897;
marijnstudent 2:e2af01393e16 12 double theta1;
marijnstudent 2:e2af01393e16 13 double theta2;
marijnstudent 0:121acb083576 14
marijnstudent 0:121acb083576 15 // functies die de hoeken berekend
marijnstudent 1:a93b870d3cf8 16 void getangles(double &theta_one,double &theta_two) // xy inputs
marijnstudent 0:121acb083576 17 {
marijnstudent 1:a93b870d3cf8 18 double x = 60+ 20*potleft.read();
marijnstudent 1:a93b870d3cf8 19 double y = 40+40*potright.read();
marijnstudent 0:121acb083576 20 double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
marijnstudent 2:e2af01393e16 21 double alfa = acos((2*pow(L,2)-pow(r,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
marijnstudent 0:121acb083576 22 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
marijnstudent 0:121acb083576 23 // hoeken berekenen
marijnstudent 0:121acb083576 24 theta_one = atan2(y,x)+beta;
marijnstudent 0:121acb083576 25 theta_two = pi + alfa;
marijnstudent 1:a93b870d3cf8 26 scope.set(0,x);
marijnstudent 1:a93b870d3cf8 27 scope.set(1,y);
marijnstudent 1:a93b870d3cf8 28 double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
marijnstudent 1:a93b870d3cf8 29 double yt = L*cos(theta_one)+L*cos(theta_one+theta_two);
marijnstudent 1:a93b870d3cf8 30 scope.set(2,xt);
marijnstudent 1:a93b870d3cf8 31 scope.set(3,yt);
marijnstudent 1:a93b870d3cf8 32 scope.send();
marijnstudent 1:a93b870d3cf8 33 }
marijnstudent 1:a93b870d3cf8 34
marijnstudent 1:a93b870d3cf8 35 int main()
marijnstudent 1:a93b870d3cf8 36 {
marijnstudent 1:a93b870d3cf8 37 while(true) {
marijnstudent 1:a93b870d3cf8 38 getangles(theta1,theta2);
marijnstudent 1:a93b870d3cf8 39 }
marijnstudent 1:a93b870d3cf8 40 }