AES-2015 / Mbed 2 deprecated Finalrobot

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:   mbed-rtos mbed

robot.cpp@0:f15aa1706e16, 2015-12-14 (annotated)

Committer:
BAC
Date:
Mon Dec 14 15:03:32 2015 +0000
Revision:
0:f15aa1706e16
bacon & tum

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BAC 0:f15aa1706e16 1 #include <math.h>
BAC 0:f15aa1706e16 2 #include "robot.h"
BAC 0:f15aa1706e16 3
BAC 0:f15aa1706e16 4 //Define dimension of robot
BAC 0:f15aa1706e16 5 Kinematic::Kinematic(float t1x, float t1z, float t2, float t3, float t4x, float t4z) {
BAC 0:f15aa1706e16 6 T1X = t1x;
BAC 0:f15aa1706e16 7 T1Z = t1z;
BAC 0:f15aa1706e16 8 T2 = t2;
BAC 0:f15aa1706e16 9 T3 = t3;
BAC 0:f15aa1706e16 10 T4X = t4x;
BAC 0:f15aa1706e16 11 T4Z = t4z;
BAC 0:f15aa1706e16 12 }
BAC 0:f15aa1706e16 13
BAC 0:f15aa1706e16 14 // Mapping from linear posittion to angular position
BAC 0:f15aa1706e16 15 void Kinematic::inverse(float x, float y, float z, unsigned int index){
BAC 0:f15aa1706e16 16 float k = (630.0/11.0); // radian to degree
BAC 0:f15aa1706e16 17 float R31, A1, A2, C1, X1, Y1, Z1, X3, Y3, Z3;
BAC 0:f15aa1706e16 18
BAC 0:f15aa1706e16 19 A[index] = atan2(y,x);
BAC 0:f15aa1706e16 20
BAC 0:f15aa1706e16 21 X1 = T1X*cos(A[index]);
BAC 0:f15aa1706e16 22 Y1 = T1X*sin(A[index]);
BAC 0:f15aa1706e16 23 Z1 = T1Z;
BAC 0:f15aa1706e16 24 X3 = x - (T4X*cos(A[index]));
BAC 0:f15aa1706e16 25 Y3 = y - (T4X*sin(A[index]));
BAC 0:f15aa1706e16 26 Z3 = T4Z;
BAC 0:f15aa1706e16 27
BAC 0:f15aa1706e16 28 R31 = sqrt(pow((X3-X1),2)+ pow((Y3-Y1),2) + pow((Z3-Z1),2));
BAC 0:f15aa1706e16 29 A1 = acos((pow(T2,2) + pow(R31,2) - pow(T3,2))/(2*T2*R31));
BAC 0:f15aa1706e16 30 A2 = atan((Z3-Z1)/(sqrt(pow((X3-X1),2) + pow((Y3 - Y1),2))));
BAC 0:f15aa1706e16 31 C1 = acos((pow(T2,2) + pow(T3,2) - pow(R31,2))/(2*T2*T3));
BAC 0:f15aa1706e16 32
BAC 0:f15aa1706e16 33 //printf("%f %f %f %f",R31,A1,A2,C1);
BAC 0:f15aa1706e16 34 B[index] = A1 + A2;
BAC 0:f15aa1706e16 35 C[index] = C1;
BAC 0:f15aa1706e16 36
BAC 0:f15aa1706e16 37 A[index] = (float) (A[index]*k); // A in degree
BAC 0:f15aa1706e16 38 B[index] = (float) 90.0f - (B[index]*k); // B in degree
BAC 0:f15aa1706e16 39 C[index] = (float) 180.0f - (C[index]*k); // C in degree
BAC 0:f15aa1706e16 40
BAC 0:f15aa1706e16 41 }
BAC 0:f15aa1706e16 42
BAC 0:f15aa1706e16 43 // Mapping from angular posittion to linear position
BAC 0:f15aa1706e16 44 void Kinematic::forward(float am, float bm, float cm){
BAC 0:f15aa1706e16 45 Xm = 0+am;
BAC 0:f15aa1706e16 46 Ym = 0+bm;
BAC 0:f15aa1706e16 47 Zm = 0+cm;
BAC 0:f15aa1706e16 48 }
BAC 0:f15aa1706e16 49 // Data Linker
BAC 0:f15aa1706e16 50 float Kinematic::GetA(unsigned int i){
BAC 0:f15aa1706e16 51 return (float) A[i];
BAC 0:f15aa1706e16 52 }
BAC 0:f15aa1706e16 53 float Kinematic::GetB(unsigned int i){
BAC 0:f15aa1706e16 54 return (float) B[i];
BAC 0:f15aa1706e16 55 }
BAC 0:f15aa1706e16 56 float Kinematic::GetC(unsigned int i){
BAC 0:f15aa1706e16 57 return (float) C[i];
BAC 0:f15aa1706e16 58 }
BAC 0:f15aa1706e16 59 float Kinematic::GetXm(void){
BAC 0:f15aa1706e16 60 return (float) Xm;
BAC 0:f15aa1706e16 61 }
BAC 0:f15aa1706e16 62 float Kinematic::GetYm(void){
BAC 0:f15aa1706e16 63 return (float) Ym;
BAC 0:f15aa1706e16 64 }
BAC 0:f15aa1706e16 65 float Kinematic::GetZm(void){
BAC 0:f15aa1706e16 66 return (float) Zm;
BAC 0:f15aa1706e16 67 }
BAC 0:f15aa1706e16 68
BAC 0:f15aa1706e16 69
BAC 0:f15aa1706e16 70