/* * moveMech.h * * Created on: Nov 29, 2018 * Author: Sam Malicoat */ #ifndef SRC_MOVEMECH_H_ #define SRC_MOVEMECH_H_ #include #include #include #include typedef float Mat[3][3]; typedef float Vector[2][2]; typedef float Rotation[2]; typedef float Motor[4]; #define W 23 /*width of robot(x axis) cm*/ #define L 26 /*length of robot(y axis) cm*/ #define FL 0 #define FR 1 #define BL 3 #define BR 2 #define pi 3.141592653589793 #define SQRT2 1.4142135623730905 #define RADIUS 17.677669529663689 /*sqrt((W/2)^2+(L/2)^2)*/ #define f_abs(a) ((a<0)?a*-1:a) #define R 6 /*centimeters RADIUS of mecanum wheel*/ #define r 1.2 /*centimeters RADIUS of mecanum roller*/ #define EPS 0.001/*difference between two floats that is concideered the same*/ #define WHEELR (R+r) /* * Used to intialize the system, needs a paramater of what FREQUENCY your *control loop is running at ie we are running at 500Hz so we pass the *paramater of 500 to the function */ void initMech(int deltaT); /* * Sets the new goal for our robot to drive to, takes a Vector (this is a float[2][2]) * t[0][0] is the x distance * t[1][1] is the y distance * t[0][1]/t[1][1] is not used by this function * Roation is a float[2] that is desired roation in degree wanted set the roation wanted * in rottemp[0] */ void setGoal(Vector t,Rotation rottemp); /* * Given the current motor speed in a Motor structure (float[4]) curSpeed * return the RELATIVE motor speed in a Motor structure (float[4]) goalSpeed * this return value is relative speed. where 1 is max speed and 0 is off. */ void getNewSpeed(volatile Motor curSpeed,volatile Motor goalSpeed); /* * Get the current position the algorithum has calculated * THis function is not nessasary to be called, on if you are interested */ void getPos(Vector xy, Rotation angle); #endif /* SRC_MOVEMECH_H_ */