void initFull(){
//setup piob for stepper motor output
at91_pio_open( &PIOB_DESC,
PB0 | PB1 | PB2 | PB3 | PB4 | PB5 | PB6 | PB7,
PIO_OUTPUT );
//initialize stepper lines
at91_pio_write( &PIOB_DESC,
CW | STEPPER_RESET | CONTROL,
HIGH);
at91_pio_write( &PIOB_DESC,
FULL| EN_AZI | EN_ELE | CLK_AZI | CLK_ELE,
LOW);
at91_pio_write( &PIOB_DESC, STEPPER_RESET, LOW );
wait( SEC/2 );
at91_pio_write( &PIOB_DESC, STEPPER_RESET, HIGH );
MODE = HALFSTEP;
azimuthStatus.braked = 0;
azimuthStatus.steps = 0;
elevationStatus.braked = 0;
elevationStatus.steps = -10;
}
void updateLEDStatus() {
if (bytecodeExecutionLedMask != 0) {
count = count + 1;
if (count % 500 == 0) {
//iprintf("Count: %i\n", count);
if (led_is_on) {
at91_pio_write (&PIO_DESC, bytecodeExecutionLedMask, PIO_SET_OUT );
led_is_on = 0;
} else {
at91_pio_write (&PIO_DESC, bytecodeExecutionLedMask, PIO_CLEAR_OUT );
led_is_on = 1;
}
}
}
}
void unbrake( u_int motor ){
at91_pio_write( &PIOB_DESC, motor, LOW );
if( motor == AZI )
azimuthStatus.braked = 0;
else
elevationStatus.braked = 0;
return;
}
void brake( u_int motor ){
at91_pio_write( &PIOB_DESC, motor, HIGH );
if( motor == AZI )
azimuthStatus.braked = 1;
else
elevationStatus.braked = 1;;
return;
}
void accelStep( u_int steps, u_int motor ){
/* Same as step except accelSteps steps at the beginning of the move
and accelSteps at the end of the move will be
accelleration/decelleration.
steps/5 = 20% of the steps
steps/4 = 25%
*/
u_int accelSteps = steps/5;
u_int i, clk, divisor, accelerator, accelDelta, period;
if (motor == AZI){
divisor = DIV_AZI;
clk = CLK_AZI;
}
else {
divisor = DIV_ELE;
clk = CLK_ELE;
}
period = SEC/divisor;
accelerator = period / 2;
accelDelta = accelerator / accelSteps;
for(i = 0 ; i < steps; i++){
if ( i <= accelSteps ){ //in acceleration mode
period = accelerator;
accelerator = accelerator + accelDelta;
}
else if ( i >= steps - accelSteps ){
period = accelerator;
accelerator = accelerator - accelDelta;
}
else
period = SEC/divisor;
at91_pio_write( &PIOB_DESC, clk, HIGH );
wait( period );
at91_pio_write( &PIOB_DESC, clk, LOW );
wait( period );
}
return;
}
void step( u_int steps, u_int motor){
/* toggle the clock line a number af times. order for this to do
anything the motor needs to be in 'braked' mode.
*/
u_int i, divisor, clk, period;
if (motor == AZI){
divisor = DIV_AZI;
clk = CLK_AZI;
}
else {
divisor = DIV_ELE;
clk = CLK_ELE;
}
period = SEC/divisor;
for(i = 0 ; i < steps; i++){
at91_pio_write( &PIOB_DESC, clk, HIGH );
wait( period );
at91_pio_write( &PIOB_DESC, clk, LOW );
wait( period );
}
return;
}
void setDirection( u_int motor, u_int dir ) {
at91_pio_write( &PIOB_DESC, CW, dir );
return;
}
/* Synchronize the camera for use Opens the usart, then goes through
* the SYNC->, <-ACK SYNC, ACK-> procedure with the camera Returns
* true on successful synchronization, false otherwise.
*/
u_int sync_camera ( CameraDesc * camera_desc )
{
u_int i, status, return_val = FALSE, period;
char * buffer;
CommandFrame sync = { HEAD, SYNC, EMPTY, EMPTY, EMPTY, EMPTY };
CommandFrame ack = { HEAD, ACK , SYNC , EMPTY, EMPTY, EMPTY };
CommandFrame rec_ack = { EMPTY, EMPTY, EMPTY, EMPTY, EMPTY, EMPTY };
CommandFrame rec_sync = { EMPTY, EMPTY, EMPTY, EMPTY, EMPTY, EMPTY };
period = DELAY; /*time between transmissions: 1/4s*/
/*Create the buffer*/
buffer = malloc ( 2 * CMD_SIZE ); /*oversized buffer in case of offset*/
memset ( buffer, 0, 2 * CMD_SIZE );
/*Set the receive multiplexer to the camera we're using*/
at91_pio_write (& PIOB_DESC, PB18, camera_desc->camera );
/*Open the usart*/
at91_usart_open ( camera_desc->usart_desc,
US_ASYNC_MODE,
camera_desc->baud_rate, 0 );
/*Set up the receive buffer*/
at91_usart_receive_frame ( camera_desc->usart_desc,
buffer,
2 * CMD_SIZE,
0 );
/*Send the sync command*/
/*Camera needs sync sent up to 60 times to detect baudrate*/
for ( i = 0 ; i < SYNC_ATTEMPTS ; i++ )
{
/*Send SYNC*/
send_command ( camera_desc, & sync );
/*Delay before next attempt*/
wait( period );
status = at91_usart_get_status ( camera_desc->usart_desc );
#ifdef DEBUG
wait( period );
#endif
/*Check if something was received*/
if ( status & ( US_ENDRX | US_TIMEOUT ) )
{
#ifdef DEBUG
wait( period );
#endif
if ( buffer[0] == 0 && buffer[1] == 0 && buffer[2] == 0 &&
buffer[3] == 0 && buffer[4] == 0 && buffer[5] == 0 ){
free( buffer );
return FALSE;
}
/*Try to get the frames*/
if ( get_frame ( buffer, & rec_ack, 2 * CMD_SIZE, 1 )
&& get_frame ( buffer, & rec_sync, 2 * CMD_SIZE, 2 ) )
{
if ( rec_ack.command == ACK
&& rec_ack.param1 == SYNC
&& rec_sync.command == SYNC )
{
/*Send ACK*/
send_command ( camera_desc, & ack );
return_val = TRUE;
break;
}
}
}
}
free ( buffer );
return return_val;
}
