//to get the gernike moments of order p and repetition q
double* zernike::CalculateZernike(int p, int q )
{
double * V = (double*) malloc (2* sizeof(double ));
V[0]=0.0;V[1]=0.0;
double** ro = RHO() ;
double** theta = THETA();
int s = (p-abs(q))/2;
for(int i =0; i<M;i++)
{
for(int j=0;j<N;j++)
{
double temp = 0.0;
if(ro[i][j]<=1.0)
{
for(int k = 0 ; k<=s;k++)
{
temp += B(p,q,k)*pow(ro[i][j],p-2*k);
}
V[0] +=temp*cos(theta[i][j]*q)*f[i][j];
V[1] -=temp*sin(theta[i][j]*q)*f[i][j];
}
else
{
V[0] += 0.0;
V[0] += 0.0;
}
}
}
double b = G00();
V[0] = V[0]*(p+1)/(3.14259265*b);
V[1] = V[1]*(p+1)/(3.14259265*b);
free(ro);
free(theta);
return V;
}
double zernike::COF_Y()
{
double Moments=0.0;
double* Y = getY();
for(int i=0;i<M;++i)
{
for(int j=0;j<N;++j)
{
Moments+=((f[i][j])*Y[i]);
}
}
free(Y);
Moments=Moments/G00();
return Moments;
}
double zernike::COF_X()
{
double Moments=0.0;
double* X =getX();
for(int i=0;i<M;++i)
{
for(int j=0;j<N;++j)
{
Moments+=((f[i][j])*X[j]);
}
}
free(X);
Moments=Moments/G00();
return Moments;
}
int main(void)
{
USART_Init(MYUBBR); //initialise UART0
uart_SendString("Set Up...\n\r");
speed_cntr_Init_Timer1();
sm_driver_Init_IO();
XLocation = 0;
YLocation = 0;
ZLocation = 0;
sei();
DDRA = 0xFF;//set port c to outputs
PORTA = 0x00;
DDRB = 0xFF;
PORTB = 0x00;
DDRC = 0xFF;
PORTC = 0x00;
DDRD = 0xFF;
PORTD = 0x00;
int Step;
unsigned int accel, decel, speed;
Step = 100;
accel = 71;
speed = 12;
decel = 71;
//int point = 0;
uart_SendString("Ready!\n\r");
while(1)
{
if(COMMANDRECEIVED == TRUE)
{
switch(Received[0])
{
case 'G':
switch(Received[1])
{
case '0':
switch(Received[2])
{
case '0':
G00();
break;
default:
uart_SendString("Unsupported G Code");
break;
}
break;
default:
uart_SendString("Unsupported G Code");
break;
}
break;
case 'g': //start all set up motors.
StartMotors();
break;
case 'X': //Set up the X motor with current settings
speed_cntr_SetupX(Step, accel, decel, speed);
break;
case 'Y': //set up the Y motor with current settings
speed_cntr_SetupY(Step, accel, decel, speed);
break;
case 'Z': //set up the Z motor with current settings
speed_cntr_SetupZ(Step, accel, decel, speed);
break;
case 'S'://set the Step variable
Step = atoi((char const *)Received+2);
break;
case 'a'://set the Accel Variable
accel = atoi((char const *)Received+2);
break;
case 'd'://set up the deceleration variable
decel = atoi((char const *)Received+2);
break;
case 's'://set the steps variable
speed = atoi((char const *)Received+2);
//speed_cntr1_Setup1(Step, accel, decel, speed);
if (speed > MAXSPEED)
{
uart_SendString("WARNING: EXCEEDED MAXIMUM SPEED");
}
break;
case 'I'://show all variable information
uart_SendString("\n\rSteps: ");
uart_SendInt(Step);
uart_SendString("\n\rAccel: ");
uart_SendInt(accel);
uart_SendString("\n\rSpeed: ");
uart_SendInt(speed);
uart_SendString("\n\rDecel: ");
uart_SendInt(decel);
break;
case 'L'://show current locations of device
USART_Transmit('X');
USART_Transmit('=');
uart_SendInt(XLocation);
USART_Transmit('\r');
USART_Transmit('\n');
USART_Transmit('Y');
USART_Transmit('=');
uart_SendInt(YLocation);
USART_Transmit('\r');
USART_Transmit('\n');
USART_Transmit('Z');
USART_Transmit('=');
uart_SendInt(ZLocation);
USART_Transmit('\r');
USART_Transmit('\n');
break;
case '0'://Zero all Variables.
XLocation = 0;
YLocation = 0;
ZLocation = 0;
break;
default://unsupported command
uart_SendString("Incorrect Command");
break;
}
COMMANDRECEIVED = FALSE;
FlushBuffer();
uart_SendString("\n\r>");
}
else
{
PORTA = PORTA;
}
}
}