Numero Uno / Mbed 2 deprecated PID_VelocityExample

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

Point.cpp@14:102a2b4f5c86, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 14:17:11 2015 +0000
Revision:
14:102a2b4f5c86
Parent:
9:07189a75e979 
integrating xy to angle calculations, not working yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 9:07189a75e979 1 // Example: data structs and functions
ewoud 9:07189a75e979 2 #include <stdio.h>
ewoud 9:07189a75e979 3 #include <math.h>
ewoud 9:07189a75e979 4
ewoud 9:07189a75e979 5 const double M_PI =3.141592653589793238463;
ewoud 9:07189a75e979 6 const float l = 10; // distance between the motors
ewoud 9:07189a75e979 7 const float armlength=15; // length of the arms from the motor
ewoud 9:07189a75e979 8
ewoud 9:07189a75e979 9 class Point
ewoud 9:07189a75e979 10 {
ewoud 9:07189a75e979 11 public:
ewoud 9:07189a75e979 12
ewoud 9:07189a75e979 13 float posX;
ewoud 9:07189a75e979 14 float posY;
ewoud 9:07189a75e979 15 float rotA;
ewoud 9:07189a75e979 16 float rotB;
ewoud 9:07189a75e979 17
ewoud 9:07189a75e979 18 bool fromCarthesian(float x, float y)
ewoud 9:07189a75e979 19 {
ewoud 9:07189a75e979 20 posX = x;
ewoud 9:07189a75e979 21 posY = y;
ewoud 9:07189a75e979 22 rotA = atan(y/x)*180/M_PI;
ewoud 9:07189a75e979 23 rotB = atan(y/(l-x))*180/M_PI;
ewoud 9:07189a75e979 24
ewoud 9:07189a75e979 25 // function is done, return the struct type Point:
ewoud 9:07189a75e979 26 return true;
ewoud 9:07189a75e979 27 }
ewoud 9:07189a75e979 28 bool fromRotational(float a, float b)
ewoud 9:07189a75e979 29 {
ewoud 14:102a2b4f5c86 30 rotA = a;
ewoud 14:102a2b4f5c86 31 rotB = b;
ewoud 14:102a2b4f5c86 32 posX = (tan(rotB*M_PI/180)*l)/(tan(rotA*M_PI/180)+tan(rotB*M_PI/180));
ewoud 14:102a2b4f5c86 33 posY = tan(rotA*M_PI/180)*posX;
ewoud 9:07189a75e979 34
ewoud 9:07189a75e979 35 return true;
ewoud 9:07189a75e979 36
ewoud 9:07189a75e979 37 }
ewoud 9:07189a75e979 38 bool checkbounds()
ewoud 9:07189a75e979 39 {
ewoud 9:07189a75e979 40 if (rotA <= 0 or rotB <= 0){
ewoud 9:07189a75e979 41 return 0;
ewoud 9:07189a75e979 42 }
ewoud 9:07189a75e979 43 if (rotA > 90 or rotB > 90){
ewoud 9:07189a75e979 44 return 0;
ewoud 9:07189a75e979 45 }
ewoud 9:07189a75e979 46 if (sqrt(pow(posX,2)+pow(posY,2)) > armlength){ // too far from left arm
ewoud 9:07189a75e979 47 return 0;
ewoud 9:07189a75e979 48 }
ewoud 9:07189a75e979 49 if (sqrt(pow(l-posX,2)+pow(posY,2)) > armlength){ // too far from right arm
ewoud 9:07189a75e979 50 return 0;
ewoud 9:07189a75e979 51 }
ewoud 9:07189a75e979 52 return true;
ewoud 9:07189a75e979 53 }
ewoud 9:07189a75e979 54 };
ewoud 9:07189a75e979 55
ewoud 9:07189a75e979 56
ewoud 9:07189a75e979 57