NQC Program
/********************************************************
following program demonstrates how to move motors, and also
provides all the functions required for this functionality.
********************************************************/
#define float 0x00
#define fwd 0x01
#define rev 0x02
#define brk 0x03
#define MTR_1 1
#define MTR_2 2
#define MTR_3 3
#define MTR_4 4
#define READ_BYTE 0x52 //R or r
#define WRITE_BYTE 0x57 //W or w
#define ABORT 0xff // 0xff
#define ALIVE 0x41 // A
#define ERROR -1
#define NO_ERROR 1
int Error;
int speed;
task main()
{
speed=250 ;
SetTxPower(TX_POWER_LO) ;
Set_motor(0xb4,MTR_1, speed,fwd);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_12(0xb4, speed,fwd,speed,rev);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_34(0xb4, speed,rev,speed,fwd);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_34(0xb4, speed,fwd,speed,rev);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_1234(0xb4, speed,brk,
speed,brk,
speed,brk,
speed,brk);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_1234(0xb4, speed,fwd,
speed,fwd,
speed,fwd,
speed,fwd);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_1234(0xb4, speed,rev,
speed,rev,
speed,rev,
speed,rev);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
Set_motor_1234(0xb4,speed,brk,
speed,brk,
speed,brk,
speed,brk);
Wait(500);
PlaySound(SOUND_DOUBLE_BEEP);
}
/********************************************************
set_motor(int Address, int motor,int speed,int direction)
Address = IIC device address eg for 24C02 will be 0xA0
motor = 1 to 4
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor(int Address, int motor,int speed,int direction)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55); // I2C protocol headers.
SetSerialData(1,0xff); // I2C protocol headers.
SetSerialData(2,0xaa); // I2C protocol headers.
SetSerialData(3,WRITE_BYTE); // I2C command
SetSerialData(4,Address);
SetSerialData(5,2*motor-1); // motormux is an I2C device, and uses
// register 2*m-1 to address each motor.
// where m is the motor number
SetSerialData(6,0x02); // no of bytes to be written
SetSerialData(7,direction);
SetSerialData(8,speed);
SendSerial(0,9);
Wait(1);
if(Message()!=2 ) Error=ERROR;
}
/********************************************************
set_motor_12(int Address, int speed_1, int direction_1,
int speed_2,int direction_2)
Address = IIC device address eg for 24C02 will be 0xA0
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor_12(int Address, int speed_1,
int direction_1,int speed_2,int direction_2)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x01); //first motor register no
SetSerialData(6,0x04); // no of bytes to be written
SetSerialData(7,direction_1);
SetSerialData(8,speed_1);
SetSerialData(9,direction_2);
SetSerialData(10,speed_2);
SendSerial(0,11);
Wait(1);
if(Message()!=4 ) Error=ERROR;
}
/********************************************************
set_motor_34(int Address, int speed_1,
int direction_1, int speed_2,int direction_2)
Address = IIC device address eg for 24C02 will be 0xA0
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor_34(int Address, int speed_1,
int direction_1,int speed_2,int direction_2)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x05); //first motor register no
SetSerialData(6,0x04); // no of bytes to be written
SetSerialData(7,direction_1);
SetSerialData(8,speed_1);
SetSerialData(9,direction_2);
SetSerialData(10,speed_2);
SendSerial(0,11);
Wait(1);
if(Message()!=4 ) Error=ERROR;
}
/********************************************************
set_motor_23(int Address, int speed_1,
int direction_1, int speed_2,int direction_2)
Address = IIC device address eg for 24C02 will be 0xA0
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor_23(int Address, int speed_1,
int direction_1,int speed_2,int direction_2)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x03); //motor register no
SetSerialData(6,0x04); // no of bytes to be written
SetSerialData(7,direction_1);
SetSerialData(8,speed_1);
SetSerialData(9,direction_2);
SetSerialData(10,speed_2);
SendSerial(0,11);
Wait(1);
if(Message()!=4 ) Error=ERROR;
}
/********************************************************
set_motor_123(int Address, int speed_1,
int direction_1, int speed_2,
int direction_2, int speed_3, int direction_3)
Address = IIC device address eg for 24C02 will be 0xA0
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor_123(int Address, int speed_1, int direction_1,
int speed_2, int direction_2,
int speed_3, int direction_3)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x01); //first motor register no
SetSerialData(6,0x06); // no of bytes to be written
SetSerialData(7,direction_1);
SetSerialData(8,speed_1);
SetSerialData(9,direction_2);
SetSerialData(10,speed_2);
SetSerialData(11,direction_3);
SetSerialData(12,speed_3);
SendSerial(0,13);
Wait(1);
if(Message()!=6 ) Error=ERROR;
}
/********************************************************
set_motor_1234(int Address, int speed_1, int direction_1,
int speed_2, int direction_2,
int speed_3, int direction_3,
int speed_4, int direction_4)
Address = IIC device address eg for 24C02 will be 0xA0
speed = 1 to 0xff 1 slow 0xff fast
direction is off fwd rev or break
make sure that you do not exceed the count than your array size
returns the bytes or Integers written as Message
// Write count no of from IIC_Data bytes to the IIC device named Address
at location Register
***********************************************************/
void Set_motor_1234(int Address, int speed_1, int direction_1,
int speed_2, int direction_2,
int speed_3, int direction_3,
int speed_4, int direction_4)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x01); //first motor register no
SetSerialData(6,0x08); // no of bytes to be written
SetSerialData(7,direction_1);
SetSerialData(8,speed_1);
SetSerialData(9,direction_2);
SetSerialData(10,speed_2);
SetSerialData(11,direction_3);
SetSerialData(12,speed_3);
SetSerialData(13,direction_4);
SetSerialData(14,speed_4);
SendSerial(0,15);
Wait(1);
if(Message()!=8 ) Error=ERROR;
}
/********************************************************
Change_add(int Address, int New_address)
Address = IIC device address eg for 24C02 will be 0xA0
New_address = new IIC device address
***********************************************************/
void Change_add(int Address, int New_address)
{
int temp;
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,WRITE_BYTE);
SetSerialData(4,Address);
SetSerialData(5,0x00);
SetSerialData(6,0x04);
SetSerialData(7,0xa0);
SetSerialData(8,0xaa);
SetSerialData(9,0xa5);
SetSerialData(10,New_address);
SendSerial(0,11);
Wait(1);
if(Message()!=2 ) Error=ERROR;
}
/********************************************************
Check_alive()
checks if the RCX2I2C is alive
Sets ERROR flag
***********************************************************/
void Check_alive()
{
ClearMessage();
SetSerialComm(SERIAL_COMM_DEFAULT);
SetSerialPacket(SERIAL_PACKET_DEFAULT);
SetSerialData(0,0x55);
SetSerialData(1,0xff);
SetSerialData(2,0xaa);
SetSerialData(3,0x41);
SendSerial(0,4);
Wait(100);
if(Message()!=ALIVE+1 ) Error=ERROR;
}
|
![[home]](themes/winzor/images/ms-logo.png){kind=logo}
