#include #include #include #include //define values of IrDA codes send from used remote control#define MUTE_BUTTON 0x807F906F#define STANDBY_ON 0x807F20DF#define VOL_UP 0x807F609F#define VOL_DOWN 0x807FA05F#define DVD 0x807F08F7#define TV 0x807F8877#define GAME 0x807F48B7#define CD 0x807FC837 //define of characters used for control of serial communication [0-8]#define SETBACKOFF 48#define SETBACKON 49#define SETPOSITIONS 50#define PRINTPOS 51#define STOPPRINTPOS 52#define SWITCHIRDA 53#define SETPOSITIONSINMS 54#define SWITCHIRDAOFF 55#define GEPOSITION 56//defines of LCD pin numbers, most probably they dont have to be changed except of I2C_ADDR which value is neccessary and have to be changed.#define I2C_ADDR 0x27#define LCD 1#define IrDA 1#define BACKLIGHT_PIN 3#define En 2#define Rw 1#define Rs 0#define D4 4#define D5 5#define D6 6#define D7 7//MIN and MAX PWM pulse sizes, they can be found in servo documentation#define MAX 2200#define MIN 800 //Positions of servos mounted in opposite direction#define INV1 1#define INV2 3#define INV3 5 //constants for computation of positions of connection points#define pi 3.14159#define deg2rad 180/pi#define deg30 pi/6//variables used for proper show of positions on LCDchar shown=0, showPos=0, useIrda=0;unsigned long time; //variable to store connected LCDLiquidCrystal_I2C lcd(I2C_ADDR, En, Rw, Rs, D4,D5,D6,D7);//IrReciever variablesIRrecv irrecv(12); // Receive Ir on digital pin n 12decode_results results; //Array of servo objectsServo servo[6];//Zero positions of servos, in this positions their arms are perfectly horizontal, in usstatic int zero[6]={1475,1470,1490,1480,1460,1490};//In this array is stored requested position for platform - x,y,z,rot(x),rot(y),rot(z)static float arr[6]={0,0.0,0, radians(0),radians(0),radians(0)};//Actual degree of rotation of all servo arms, they start at 0 - horizontal, used to reduce//complexity of calculating new degree of rotationstatic float theta_a[6]={0.0,0.0,0.0, 0.0,0.0,0.0};//Array of current servo positions in usstatic int servo_pos[6];//rotation of servo arms in respect to axis xconst float beta[] = {pi/2,-pi/2,-pi/6, 5*pi/6,-5*pi/6,pi/6},//maximum servo positions, 0 is horizontal position servo_min=radians(-80),servo_max=radians(80),//servo_mult - multiplier used for conversion radians->servo pulse in us//L1-effective length of servo arm, L2 - length of base and platform connecting arm//z_home - height of platform above base, 0 is height of servo armsservo_mult=400/(pi/4),L1 = 0.79,L2 = 4.66, z_home = 4.05;//RD distance from center of platform to attachment points (arm attachment point)//RD distance from center of base to center of servo rotation points (servo axis)//theta_p-angle between two servo axis points, theta_r - between platform attachment points//theta_angle-helper variable//p[][]=x y values for servo rotation points//re[]{}=x y z values of platform attachment points positions//equations used for p and re will affect postion of X axis, they can be changed to achieve//specific X axis positionconst float RD = 2.42,PD =2.99,theta_p = radians(37.5),theta_angle=(pi/3-theta_p)/2, theta_r = radians(8), p[2][6]={ { -PD*cos(deg30-theta_angle),-PD*cos(deg30-theta_angle), PD*sin(theta_angle),PD*cos(deg30+theta_angle), PD*cos(deg30+theta_angle),PD*sin(theta_angle) }, { -PD*sin(deg30-theta_angle),PD*sin(deg30-theta_angle), PD*cos(theta_angle),PD*sin(deg30+theta_angle), -PD*sin(deg30+theta_angle),-PD*cos(theta_angle) } }, re[3][6] = { { -RD*sin(deg30+theta_r/2),-RD*sin(deg30+theta_r/2), -RD*sin(deg30-theta_r/2),RD*cos(theta_r/2), RD*cos(theta_r/2),-RD*sin(deg30-theta_r/2), },{ -RD*cos(deg30+theta_r/2),RD*cos(deg30+theta_r/2), RD*cos(deg30-theta_r/2),RD*sin(theta_r/2), -RD*sin(theta_r/2),-RD*cos(deg30-theta_r/2), },{ 0,0,0,0,0,0 }};
Posted on: Fri, 19 Dec 2014 06:55:16 +0000
Trending Topics
Recently Viewed Topics
© 2015