//*!!Sensor, S1, NRLINK, sensorI2CCustomStd9V, , !!*// //*!!Sensor, S2, frontSonar, sensorSONAR9V, , !!*// //*!!Sensor, S3, rearSonar, sensorSONAR9V, , !!*// //*!! !!*// //*!!Start automatically generated configuration code. !!*// const tSensors NRLINK = (tSensors) S1; //sensorI2CCustomStd9V //*!!!!*// const tSensors leftSonar = (tSensors) S2; //sensorSONAR9V //*!!!!*// const tSensors rightSonar = (tSensors) S3; //sensorSONAR9V //*!!!!*// //*!!CLICK to edit 'wizard' created sensor & motor configuration. !!*// /* *This is sample program to use with mindsensors.com NRLink interface. *V1.1 * * * Written by Dr. Nitin Patil * Copyright (c) mindsensors.com 2006, 2007 * for more info visit www.mindsensors.com * * Modified by Mark Crosbie mark@mastincrosbie.com 1 April 2008 * To install macros that power both motors simultaneously * For more information visit http://markclego.mastincrosbie.com * * This sample has been updated to demonstrate * the use of NRLink to drive PF Motors * */ /* Changeable macro definitions of NRLink address definition RCX opcode 0x50 Motor Ch1 A Float, 02, 01, 00, 0x53 Motor Ch1 A Forward, 02, 01, 10, 0x56 Motor Ch1 A Reversed, 02, 01, 20, 0x59 Motor Ch1 A Brake, 02, 01, 30, 0x5C Motor Ch1 B Float, 02, 01, 00, 0x5F Motor Ch1 B Forward, 02, 01, 40, 0x62 Motor Ch1 B Reversed, 02, 01, 80, 0x65 Motor Ch1 B Brake, 02, 01, c0, 0x68 Motor Ch2 A Float, 02, 11, 00, 0x6B Motor Ch2 A Forward, 02, 11, 10, 0x6E Motor Ch2 A Reversed, 02, 11, 20, 0x71 Motor Ch2 A Brake, 02, 11, 30, 0x74 Motor Ch2 B Float, 02, 11, 00, 0x77 Motor Ch2 B Forward, 02, 11, 40, 0x7A Motor Ch2 B Reversed, 02, 11, 80, 0x7D Motor Ch2 B Brake, 02, 11, c0, 0x80 Motor Ch3 A Float, 02, 21, 00, 0x83 Motor Ch3 A Forward, 02, 21, 10, 0x86 Motor Ch3 A Reversed, 02, 21, 20, 0x89 Motor Ch3 A Brake, 02, 21, 30, 0x8C Motor Ch3 B Float, 02, 21, 00, 0x8F Motor Ch3 B Forward, 02, 21, 40, 0x92 Motor Ch3 B Reversed, 02, 21, 80, 0x95 Motor Ch3 B Brake, 02, 21, c0, 0x98 Motor Ch4 A Float, 02, 31, 00, 0x9B Motor Ch4 A Forward, 02, 31, 10, 0x9E Motor Ch4 A Reversed, 02, 31, 20, 0xA1 Motor Ch4 A Brake, 02, 31, 30, 0xA4 Motor Ch4 B Float, 02, 31, 00, 0xA7 Motor Ch4 B Forward, 02, 31, 40, 0xAA Motor Ch4 B Reversed, 02, 31, 80, 0xAD Motor Ch4 B Brake, 02, 31, C0, Motor Ch1 A Forw B Forw, B0, 02, 01, 50, Motor Ch1 A Forw B Rev, B3, 02, 01, 90, Motor Ch1 A Rev B Forw, B6, 02, 01, 60, Motor Ch1 A Rev B Rev, B9, 02, 01, a0, Motor Ch2 A Forw B Forw, BC, 02, 11, 50, Motor Ch2 A Forw B Rev, BF, 02, 11, 90, Motor Ch2 A Rev B Forw, C2, 02, 11, 60, Motor Ch2 A Rev B Rev, C5, 02, 11, a0, Motor Ch3 A Forw B Forw, C8, 02, 21, 50, Motor Ch3 A Forw B Rev, CB, 02, 21, 90, Motor Ch3 A Rev B Forw, CE, 02, 21, 60, Motor Ch3 A Rev B Rev, D1, 02, 21, a0, Motor Ch4 A Forw B Forw, D4, 02, 31, 50, Motor Ch4 A Forw B Rev, D7, 02, 31, 90, Motor Ch4 A Rev B Forw, DA, 02, 31, 60, Motor Ch4 A Rev B Rev, DD, 02, 31, a0 */ //definations for NRLink const ubyte NRLinkID = 0x02; const ubyte NRLinkDataBytes = 0x40; const ubyte NRLinkCommandReg = 0x41; const ubyte NRLinkReadResult = 0x42; const ubyte NRLinkWriteData = 0x42; const tSensors NRLinkPort = NRLINK; // Connect NRLink sensor to this port!! const ubyte NRLinkDefault = 0x44; const ubyte NRLinkFlush = 0x46; const ubyte NRLinkHighSpeed = 0x48; const ubyte NRLinkLongRange = 0x4C; const ubyte NRLinkShortRange = 0x53; const ubyte NRLinkSetADPAON = 0x4E; const ubyte NRLinkSETADPAOFF = 0x4F; const ubyte NRLinkTxUnassembled = 0x55; const ubyte NRLinkSelectRCX = 0x58; const ubyte NRLinkSelectTRAIN = 0x54; const ubyte NRLinkSelectPF = 0x50; const ubyte NRLinkMacro = 0x52; const ubyte Macro_Short_range = 0x01; const ubyte Macro_Long_Range = 0x04; const ubyte Motor_Ch1_A_Float = 0x50; const ubyte Motor_Ch1_A_FWD = 0x53; const ubyte Motor_Ch1_A_REV = 0x56; const ubyte Motor_Ch1_A_Brake = 0x59; const ubyte Motor_Ch1_B_Float = 0x5C; const ubyte Motor_Ch1_B_FWD = 0x5F; const ubyte Motor_Ch1_B_REV = 0x62; const ubyte Motor_Ch1_B_Brake = 0x65; const ubyte Motor_Ch2_A_Float = 0x68; const ubyte Motor_Ch2_A_FWD = 0x6B; const ubyte Motor_Ch2_A_REV = 0x6E; const ubyte Motor_Ch2_A_Brake = 0x71; const ubyte Motor_Ch2_B_Float = 0x74; const ubyte Motor_Ch2_B_FWD = 0x77; const ubyte Motor_Ch2_B_REV = 0x7A; const ubyte Motor_Ch2_B_Brake = 0x7D; const ubyte Motor_Ch3_A_Float = 0x80; const ubyte Motor_Ch3_A_FWD = 0x83; const ubyte Motor_Ch3_A_REV = 0x86; const ubyte Motor_Ch3_A_Brake = 0x89; const ubyte Motor_Ch3_B_Float = 0x8C; const ubyte Motor_Ch3_B_FWD = 0x8F; const ubyte Motor_Ch3_B_REV = 0x92; const ubyte Motor_Ch3_B_Brake = 0x95; const ubyte Motor_Ch4_A_Float = 0x98; const ubyte Motor_Ch4_A_FWD = 0x9B; const ubyte Motor_Ch4_A_REV = 0x9E; const ubyte Motor_Ch4_A_Brake = 0xA1; const ubyte Motor_Ch4_B_Float = 0xA4; const ubyte Motor_Ch4_B_FWD = 0xA7; const ubyte Motor_Ch4_B_REV = 0xAA; const ubyte Motor_Ch4_B_Brake = 0xAD; const ubyte Motor_Ch1_A_Forw_B_Forw = 0xB0; const ubyte Motor_Ch1_A_Forw_B_Rev = 0xB3; const ubyte Motor_Ch1_A_Rev_B_Forw = 0xB6; const ubyte Motor_Ch1_A_Rev_B_Rev = 0xB9; const int kThreshold = 20; // threshold for sonar sensor // extensions to the default macros loaded into the NRlink to // allow for control of two motors simultaneously const ubyte powerFunctionsMacros[] = { 0xB0, 0x02, 0x01, 0x50, // Motor Ch1 A Forw B Forw 0xB3, 0x02, 0x01, 0x90, // Motor Ch1 A Forw B Rev 0xB6, 0x02, 0x01, 0x60, // Motor Ch1 A Rev B Forw 0xB9, 0x02, 0x01, 0xa0, // Motor Ch1 A Rev B Rev 0xBC, 0x02, 0x11, 0x50, // Motor Ch2 A Forw B Forw 0xBF, 0x02, 0x11, 0x90, // Motor Ch2 A Forw B Rev 0xC2, 0x02, 0x11, 0x60, // Motor Ch2 A Rev B Forw 0xC5, 0x02, 0x11, 0xa0, // Motor Ch2 A Rev B Rev 0xC8, 0x02, 0x21, 0x50, // Motor Ch3 A Forw B Forw 0xCB, 0x02, 0x21, 0x90, // Motor Ch3 A Forw B Rev 0xCE, 0x02, 0x21, 0x60, // Motor Ch3 A Rev B Forw 0xD1, 0x02, 0x21, 0xa0, // Motor Ch3 A Rev B Rev 0xD4, 0x02, 0x31, 0x50, // Motor Ch4 A Forw B Forw 0xD7, 0x02, 0x31, 0x90, // Motor Ch4 A Forw B Rev 0xDA, 0x02, 0x31, 0x60, // Motor Ch4 A Rev B Forw 0xDD, 0x02, 0x31, 0xa0 // Motor Ch4 A Rev B Rev }; ///////////////////////////////////////////////////////////////////////////// // // send Command to NrLink interface // ///////////////////////////////////////////////////////////////////////////// void NRLinkCommand(byte NRLinkCommand) { byte NRLinkMsg[5]; const byte MsgSize = 0; const byte Address = 1; const byte CommandAddress = 2; const byte Command = 3; // Build the I2C message NRLinkMsg[MsgSize] = 3; NRLinkMsg[Address] = NRLinkID; NRLinkMsg[CommandAddress] = NRLinkCommandReg ; NRLinkMsg[Command] = NRLinkCommand; while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING) { // Wait for I2C bus to be ready } // when the I2C bus is ready, send the message you built sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0); } ///////////////////////////////////////////////////////////////////////////// // // send macro and run it from NRLink interface // ///////////////////////////////////////////////////////////////////////////// void NRLinkRunMacro(byte NRLinkMacroAdd) { byte NRLinkMsg[5]; const byte MsgSize = 0; const byte Address = 1; const byte CommandAddress = 2; const byte Command = 3; const byte MacroAddress = 4; // Build the I2C message NRLinkMsg[MsgSize] = 4; NRLinkMsg[Address] = NRLinkID; NRLinkMsg[CommandAddress] = NRLinkCommandReg; NRLinkMsg[Command] = NRLinkMacro; NRLinkMsg[MacroAddress] = NRLinkMacroAdd; while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING) { // Wait for I2C bus to be ready } // when the I2C bus is ready, send the message you built sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0); } ////////////////////////////////////////////////////////////////////////////////////////// // // Send a Message via I2C // // Sends an arbitrary 2-byte message over an I2C port. It would be easy to modify for // messages of different length. Simply adjust the function parameters and the initialization // of the 'nMsg' array. // // Usually when writing to device the reply length will be zero. // ////////////////////////////////////////////////////////////////////////////////////////// bool sendI2CMessage(tSensors nPortIndex, ubyte registerIndex, ubyte nByte1, ubyte nByte2, ubyte nByte3) { const int kI2CAddress = 0x02; // You may want to make this a function parameter const ubyte nMsg[] = { 2 + 3, // This is length field for transmitted message. kI2CAddress, // The I2C address of the device. Almost all devices use value '0x02' registerIndex, // The internal register index within the sensor to start writing at. nByte1, nByte2, nByte3 }; // wait for I2C bus to be available while (nI2CStatus[nPortIndex] == STAT_COMM_PENDING) { wait1Msec(1); } sendI2CMsg(nPortIndex, nMsg[0], 0); return true; } ////////////////////////////////////////////////////////////////////////////////////////// // // Install additional power function macros into the NRLink that allow for // simultaneously control of both attached motors // ///////////////////////////////////////////////////////////////////////////////////////// void installPFMacros() { int i, numPFmacros; nxtDisplayTextLine(2, "Writing macro"); numPFmacros = sizeof(powerFunctionsMacros); for(i=0; i < numPFmacros; i+=4) { // install macro into address sendI2CMessage(NRLinkPort, // port the NRLink is connected to powerFunctionsMacros[i], // register address to write the macro into powerFunctionsMacros[i+1], // number of bytes in the macro powerFunctionsMacros[i+2], // macro command powerFunctionsMacros[i+3]);// macro command } nxtDisplayTextLine(3, "Done"); wait10Msec(100); } void brakeMotors() { NRLinkRunMacro(Motor_Ch1_A_Brake); wait10Msec(10); NRLinkRunMacro(Motor_Ch1_B_Brake); wait10Msec(10); } ///////////////////////////////////////////////////////////////////////////// // // Run some commands and macro to control PF Motors using NRLink. // ///////////////////////////////////////////////////////////////////////////// task main() { int leftReading, rightReading; nI2CBytesReady[NRLinkPort] = 0; SensorType[NRLinkPort] = sensorI2CCustom9V; NRLinkCommand(NRLinkFlush); NRLinkCommand(NRLinkDefault); NRLinkCommand(NRLinkLongRange); NRLinkCommand(NRLinkSelectPF); //installPFMacros(); nxtDisplayTextLine(4, "Driving"); while(1) { // start driving forwards leftReading = SensorValue[leftSonar]; rightReading = SensorValue[rightSonar]; while( (leftReading > kThreshold) && (rightReading > kThreshold) ) { NRLinkRunMacro(Motor_Ch1_A_Forw_B_Rev); // motors are wired in opposite directions wait10Msec(10); leftReading = SensorValue[leftSonar]; rightReading = SensorValue[rightSonar]; } brakeMotors(); // obstacle on the right, reverse turn to the left while( (leftReading > kThreshold) && (rightReading <= kThreshold) ) { NRLinkRunMacro(Motor_Ch1_A_Forw_B_Forw); wait10Msec(10); leftReading = SensorValue[leftSonar]; rightReading = SensorValue[rightSonar]; } brakeMotors(); // obstacle on the left, reverse turn to the right while( (leftReading <= kThreshold) && (rightReading > kThreshold) ) { NRLinkRunMacro(Motor_Ch1_A_Rev_B_Rev); wait10Msec(10); leftReading = SensorValue[leftSonar]; rightReading = SensorValue[rightSonar]; } brakeMotors(); // obstacle dead ahead, reverse and turn while( (leftReading <= kThreshold) && (rightReading <= kThreshold) ) { NRLinkRunMacro(Motor_Ch1_A_Rev_B_Forw); wait10Msec(100); NRLinkRunMacro(Motor_Ch1_A_Forw_B_Forw); wait10Msec(10); leftReading = SensorValue[leftSonar]; rightReading = SensorValue[rightSonar]; } brakeMotors(); } }