problem with inverse kinematic

[muz]

hello hai..
i'm doing my project about 3 degree of freedom robot arm using PIC16f877A, 20MHz Crystal.The actuators for the arm are servos controlled by the servo controller SC16A. my servos are functioning well without applying inverse kinematic equation. i already derived the inverse kinematic and put it as a function in my programming. unfortunately, my servo doesn't receive the return value from the equation that need to be sent to servo controller by using send_cmd function . i'm a beginner in programming..i believe that the experts from this forum will help me to solve my problem..i attached the program below. several errors occurred when i build the program..plez help me..

Build C:\Documents and Settings\mUs\My Documents\mplab\fyp for device 16F877A
Using driver C:\Program Files\HI-TECH Software\PICC\PRO\9.60\bin\picc.exe

Warning [356] C:\Documents and Settings\mUs\My Documents\mplab\fyp1.c; 91.15 implicit conversion of float to integer
Warning [356] C:\Documents and Settings\mUs\My Documents\mplab\fyp1.c; 92.15 implicit conversion of float to integer
Warning [356] C:\Documents and Settings\mUs\My Documents\mplab\fyp1.c; 93.15 implicit conversion of float to integer
Warning [345] C:\Documents and Settings\mUs\My Documents\mplab\fyp1.c; 96.1 unreachable code
Warning [345] C:\Documents and Settings\mUs\My Documents\mplab\fyp1.c; 97.1 unreachable code

Error [1253] double.c; 55. could not find space (181 bytes) for auto/param block
Error [1253] C:\Program Files\HI-TECH Software\PICC\PRO\9.60\sources\float.c; 27. could not find space (181 bytes) for auto/param block

********** Build failed! **********

#include <pic.h>   					// this sample code is using 16F877A !!
#include <math.h>

//==========================================================================
__CONFIG ( 0x3F32 );				//configuration for the  microcontroller
											


#define sw1    PORTB.f0                                //switch for start program


#define servo1 0x41
#define servo2 0x42
#define servo3 0x43
double pos1; //angle servo 1
double pos2;   //2
double pos3;   //3


void UART1_Init(const unsigned long baud_rate);
double calculate(unsigned char px, unsigned char py, unsigned char pz);
void send_cmd(unsigned char num, unsigned int _data, unsigned char ramp);
void UART1_Write (char _data);
void Delay_ms(const unsigned long time_in_ms);
/*double atan2(double y, double x);
double sin(double f);
double pow(double x, double y);
double cos(double f);
double sqrt(double x);*/



void main() {



   TRISC=0b10000000;                                        //set input or output
   TRISB=0b00000001;

   UART1_Init(9600);
   Delay_ms(2000);

   calculate(0x0f,0x0f,0x0f); // required position x,y,z
   send_cmd(servo1,pos1,0);
   send_cmd(servo2,pos2,0);
   send_cmd(servo3,pos3,0);
  // Delay_ms(2000);

   //send_cmd(servo1,0x000A,0);

}
void send_cmd(unsigned char num, unsigned int _data, unsigned char ramp)
{
        unsigned char higher_byte=0, lower_byte=0;

        //position value from 0-1463 are greater than a byte
        //so needs two bytes to send
        higher_byte=(_data>>6)&0x003f;        //higher byte = 0b00xxxxxx
        lower_byte=_data&0x003f;                        //lower byte  = 0b00xxxxxx


                        UART1_Write(num);                     //First byte is the servo channel 0x41-0x60
                        UART1_Write(higher_byte);             //second byte is the higher byte of position 0b00xxxxxx
                        UART1_Write(lower_byte);              //third byte is the lower byte of position 0b00xxxxxx
                        UART1_Write(ramp);                    //fourth byte is the speed value from 0-63

}
double calculate(unsigned char px, unsigned char py, unsigned char pz) //input position x,y,z
{
   double d1;
          double d;
          double a2;
          double a3;
          double theta1,theta2,theta3;
          double G,E,F;

               d1=2;         //robot arm parameters//
               d=4;          //lenght of elbow
               a2=15;        //lenght of arm1
               a3=18;        //lenght of arm2
               
               
        //inverse kinematic equation to calculate the angles theta
        theta1=atan2 (px,py)-atan2 (sqrt(pow(px,2)+pow(py,2)-pow(d1,2)),d);
        G=(pow(px,2)+pow(py,2)+pow(pz-d,2)-pow(a2,2)-pow(a3,2))/(2*a2*a3);
        theta3=atan2 (G,sqrt(1-pow(G,2)));
        E=atan2 (sqrt(pow(px,2)+pow(py,2)),pz-d);
        F=atan2 ((a2+a3*cos(theta3)),a3*sin(theta3));
        theta2=E-F;

 pos1= 8*theta1;
 pos2= 8*theta2;
 pos3= 8*theta3;
 
 return pos1;
 return pos2;
 return pos3;
}