Code:
while(1)
{
for ( m = 0 ; m < 2500 ; m++ ) {
WingMan_CenterOut( m );
WingMan_Left( m );
}
}
// Factorise your copy/paste code!
void write_and_wait ( unsigned char addr, unsigned char *command, size_t charcount )
{
I2C_bWriteBytes(addr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
void WingMan_CenterOut(int m)
{
if(m<400)
{
unsigned char command[] = { 0x00, 0x18 };
write_and_wait( 0x21, command, sizeof(command) );
}
else if((m>400) && (m<800))
{
I2Caddr=0x21;
command[0]=0x00;
command[1]=0x24;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((m>800) && (m<1200))
{
I2Caddr=0x21;
charcount=2;
command[0]=0x00;
command[1]=0x42;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if ( m < 1600 )
{
I2Caddr=0x21;
charcount=2;
command[0]=0x00;
command[1]=0x81;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
} // end of wingMan center out
void WingMan_Right(int i)
{
if(i<300)
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x01;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>300) && (i<600))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x03;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>600) && (i<900))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x07;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>900) && (i<1200))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x0F;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>1200) && (i<1500))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x1F;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>1500) && (i<1800))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x3F;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if((i>1800) && (i<2100))
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0x7F;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
else if ( i < 2500 )
{
I2Caddr=0x20;
charcount=2;
command[0]=0x00;
command[1]=0xFF;
I2C_bWriteBytes(I2Caddr, command, charcount, I2C_CompleteXfer);
while(!(I2C_bReadI2CStatus( ) & I2CHW_WR_COMPLETE));
I2C_ClrWrStatus( );
}
} // end of wingman right