The LEGO Mindstorms NXT based Wheeled Inverted Pendulum has been proven to work using various types of sensors including stock light sensors and after market gyro sensors.

Note: If the videos do not load, just refresh the page.

The light sensor control sceme relies on a relatively solid colored texture free surface to perform well. Changes in light intensity at the sensor cause the robot to move forward or backward in response the sensed tilt. The gyro based model is more robust as it senses angular velocity of the robot directly. Being Gyro controlled, this robot can handle any colored surface, including ramps and small bumps. The gyro seen in the video about is sold by HiTechnic HiTechnic also provides an assembly manual for their “HiTechnic HTWay” and sample code that allows it to balance and be controlled by and infrared transmitting controller.

hitechnichtwayassembly.pdf, hitechnichtwaynxccode.nxc.zip

The goal of this project was to verify HiTechnic's controller using their gyro, then modify the controller to work with an analog Sparkfun 6-axis IMU (sku: SEN-09431, Retired):
===== Original HTWay =====
===== Building Instructions ===== hitechnichtwayassembly.pdf These instructions are also available from HiTechnic's Models page In DASL, we have only the HiTechnic Gyro. If you omit the IR receiver, you will have to comment out the corresponding sensor calls in the original code. The edited code in the following section will account for this omission. ===== Code (NXC) ===== hitechnichtwaynxccode.nxc.zip This code is also available from HiTechnic's Models page ===== NXTway using Sparkfun IMU ===== My method of retrofitting the Sparkfun IMU using the HiTechnic HTway design and NXC code: * Build HiTechnic design HTway * Test HiTechnic code with HiTechnic gyro (IR control omitted) * Connect IMU and log data from both the HiTechnic and Sparkfun gyros for comparison * Using both sets of data, find the equation relating IMU data to HiTechnic Gyro data so that the gyro input to the controller effectively looks the same. * Alter code to read only IMU gyro data and remove the HiTechnic gyro * Edit PID gains to account for slower update frequency. ===== Build HTway ===== Building instruction can be found on HiTechnics site or right here: hitechnichtwayassembly.pdf When building this model, you can omit the IR sensor and rubber bumpers. The IR sensor use will be omitted from the code and the rubber bumpers are unavailable in the DASL Mindstorms kits.<br><br> When adding the Sparkfun IMU into the mix, you should match the position, orientation and wiring seen in the following pictures. This way, all the code will work. You can change things up later. As seen on the underside of the IMU, the 3V in can be wired to the 3.3V output on the HT proto-board and the ground should be wired to ground. The 4x-amplified X-axis gyro output should be wired to the first analog input, A0.
===== Test HiTechnic Code w/o IR ===== Before you can run HiTechnics HTway code on your wheeled inverted pendulum, you must comment out the calls to the IR sensor. Comment out lines 273 and 511 nxtway_htgyro_noir.nxc.zip ===== IMU gyro and HT gyro data logging ===== The functions for writing data to a file can be found in the code provided below. The functions initialize a file, prevent overwriting of previously saves files, and create a comma separated CSV file usable by MS Excel or another spreadsheet program. The provided reads each gyro every iteration and stores the raw data read from each. nxtway_datalog.nxc.zip The data obtained before filtering the IMU gyro data is found to look a lot like the HT gyro data but is offset by almost 400 units and flipped about the x-axis (depending on gyro orientation).
===== IMU Data Massaging ===== To find the offset necessary to center the IMU gyro data about the y=0 axis (the same center as the HT gyro data), the average of the differences between the two data sets at each point was found (-384.525). Offset = Mean( IMUgyro_i - HTgyro_i ); where i = 1..Number of data points. Adding this value to the raw IMU gyro data at every point pulls the data down and centers it about the same central axis as the HT gyro data. After that, it can be seen that the IMU gyro data must be scaled slightly so that the local data minimums and maximums correspond in amplitude. This scaling factor can be found by examining corresponding peaks in HT gyro data and IMU gyro data. This scaling factor is found to be -0.98. This scalar to be multiplied by the offset IMU gyro data both decreases the amplitude slightly and flips the data over the x-axis. The data from both sensors now lines up and can be verified by applying filtering when the data is read in the code. IMUgyro = -0.98*(IMUgyro_raw - 384.525)
===== Wheeled Inverted Pendulum with IMU gyro ===== At this point, the gyros can effectively be swapped. This means that the HT gyro can be omitted from the control loop and removed from the robot. If you replace every part of the code where the controller calls data from the HT gyro with a read of the analog port connected to the IMU gyro, you will find that the robot almost works but is extremely unstable. This instability is due to a much slower control loop and therefore a need for a more extreme reaction. The robot will fall further before the wheels are given a command to react. While the HT gyro can be read at up to 300Hz, the low speed proto-board board connected to the IMU can only be read at about 20Hz max. Luckily, it is easy to compensate for this newly introduced lag in the controller. This compensation comes in the form of cranking up a gain on the effect of sensed angle (integrated angular velocity from gyro) on the reactionary motor speed. This increases causes smaller tilt angles to cause greater motor speeds than the original gain. With a slower reaction time, a reaction of greater magnitude is necessary to account for lost time! #define KGYROANGLE 10 originally 7.5 #define KGYROSPEED 1.15 #define KPOS 0.07 #define KSPEED 0.1 The completed code for controlling the HTway with the alternative Sparkfun razor IMU can be downloaded here: nxtway_justimu.nxc.zip Keep in mind: For this to work, the sensor should be mounted in the same orientation and at the same distance from the wheel axis. Also, the sensor should be wired to the first analog input (A0) and be wire on the same size breadboard to maintain a similar moment of inertia about the wheel axis as the my modified HTway. ===== Video ===== Note: If the videos do not load, just refresh the page.<br> Thanks for reading! ===== NXC Code ===== <code c HTWay.nxc> ===================================================================== HiTechnic HTWay Version 1.1 Copyright © 2010 HiTechnic Gyro based sample Segway type robot. This NXC program uses the LEGO 2.0 firmware which has floating point support. It has been tested with LEGO NXT firmware 1.29. To use the floating point math available in the 2.0 firmware, BricxCC must be setup to tell the compiler to target that firmware. From the Edit menu, select Preferences. Select the Compiler tab, and then NBC/NXC sub-tab and check 'NXT 2.0 compatible firmware.' Modified by Alex Alspach for use with Sparkfun Razor 6-Axis IMU. 2012. Port Input and Output #define GYRO IN_1 #define IR_RECEIVER IN_4 #define LEFT_MOTOR OUT_C #define RIGHT_MOTOR OUT_A #define MOTORS OUT_AC ===================================================================== Balancing constants These are the constants used to maintain balance. Loop wait time. WAIT_TIME is the time in ms passed to the Wait command. #define WAIT_TIME 8 Wheel ratio is relative size compared to NXT 1.0 wheels For the NXT 1.0 wheels (56×26) use: 1.0 For the NXT 2.0 wheels (43.2×22) use: 0.7 For the big white RCX wheels (81.6×15) use: 1.4 #define WHEEL_RATIO_NXT1 1.0 #define WHEEL_RATIO_NXT2 0.8 #define WHEEL_RATIO_RCX 1.4 These are the main four balance constants, only the gyro constants are relative to the wheel size. KPOS and KSPEED are self-relative to the wheel size. #define KGYROANGLE 7.5 #define KGYROSPEED 1.15 #define KPOS 0.07 #define KSPEED 0.1 #define KGYROANGLE 10 #define KGYROSPEED 1.15 #define KPOS 0.07 #define KSPEED 0.1 This constant aids in drive control. When the robot starts moving because of user control, this constant helps get the robot leaning in the right direction. Similarly, it helps bring robot to a stop when stopping. #define KDRIVE 10 Power differential used for steering based on difference of target steering and actual motor difference. #define KSTEER 0.25 Gyro offset control The gyro sensor will drift with time. This constant is used in a simple long term averaging to adjust for this drift. Every time through the loop, the current gyro sensor value is averaged into the gyro offset weighted according to this constant. #define EMAOFFSET -0.0005 If robot power is saturated (over +/- 100) for over this time limit then robot must have fallen. In milliseconds. #define TIME_FALL_LIMIT 1000 ——————————————————————— IR Control constants #define CONTROL_WAIT 25 To use a different IR channel, chage these constants #define IR_LEFT HT_CH1_A #define IR_RIGHT HT_CH1_B This constant is in degrees/second for maximum speed. Note that position and speed are measured as the sum of the two motors, in other words, 600 would actually be 300 degrees/second for each motor. #define CONTROL_SPEED 600.0 ===================================================================== Global Variable for File Handing and Gyro Data Logging unsigned int result; flag returned when handling files byte fileHandle; handle to the data file short fileSize; size of the file short bytesWritten; number of bytes written to the file string fileHeader; column header for data in the file int fileNumber, filePart; integers to split up data file names string fileName; name of the file int filesize = 30240; string strFileNumber; file number e.g myDataFile 1, 2, 3 string strFilePart; file part e.g. myDataFile1-1, 1-2, 1-3 string text; string to be written to file i.e. data values string strgyroRaw; string strgOffset; string strgyroSpeed; string strgyroAngle; string strtInterval; string strA0raw; string strA1raw; float A0raw; float A1raw; Global variables These variables are used to control the movement of the robot. Both are in degrees/second. motorControlDrive is the target speed for the sum of the two motors while motorControlSteer is the target change in difference for two motors. float motorControlDrive = 0.0; float motorControlSteer = 0.0; This global contains the target motor differential, essentially, which way the robot should be pointing. This value is updated every time through the balance loop based on motorControlSteer float motorDiffTarget = 0.0; Time that robot first starts to balance. Used to calculate tInterval long tCalcStart; tInterval is the time, in seconds, for each iteration of the balance loop. float tInterval; ratioWheel stores the relative wheel size compared to a standard NXT 1.0 wheel. RCX 2.0 wheel has ratio of 0.7 while large RCX wheel is 1.4 float ratioWheel; Gyro globals float gOffset; float gAngleGlobal = 0; Motor globals float motorPos = 0; long mrcSum = 0, mrcSumPrev; long motorDiff; long mrcDeltaP3 = 0; long mrcDeltaP2 = 0; long mrcDeltaP1 = 0; ===================================================================== #include “NXCDefs.h” #define PROTO_PORT IN_2 int inputdata; int outputdata; int count; byte cmndbuf[]; buffer for outbound I2C command byte respbuf[]; buffer for inbound I2C response void readdataA0() { ArrayInit(cmndbuf, 0, 2); set the buffer to hold 2 values cmndbuf[0] = 0x02; set write to channel cmndbuf[1] = 0x42; to set read address count=2; 2 bytes to read I2CBytes(PROTO_PORT, cmndbuf, count, respbuf); issue I2C write command and read the byte back A0raw=respbuf[0]*4+respbuf[1]; create input value by reading the high order byte, shift it left 3 bits and add the low order byt to create a full 10 bit value } void readdataA1() { ArrayInit(cmndbuf, 0, 2); set the buffer to hold 2 values cmndbuf[0] = 0x02; set write to channel cmndbuf[1] = 0x44; to set read address count=2; 2 bytes to read I2CBytes(PROTO_PORT, cmndbuf, count, respbuf); issue I2C write command and read the byte back A1raw=respbuf[0]*4+respbuf[1]; create input value by reading the high order byte, shift it left 3 bits and add the low order byt to create a full 10 bit value } Functions for File Handling and Gyro Data Logging void InitWriteToFile() { fileNumber = 0; set first data file to be zero filePart = 0; set first part of first data file to zero fileName = “myMotorSpeed.csv”; name of data file result=CreateFile(fileName, filesize, fileHandle); NXT Guide Section 9.100 pg. 1812 and Section 6.59.2.2 pg. 535 returns file handle (unsigned int) check if the file already exists while (result==LDR_FILEEXISTS) LDR_FILEEXISTS is returned if file pre-exists { CloseFile(fileHandle); fileNumber = fileNumber + 1; if data file already exists, create new one fileName=NumToStr(fileNumber); fileName=StrCat(“myMotorSpeed”, fileName, “.csv”); result=CreateFile(fileName, filesize, fileHandle); } end while play a tone every time a file is created PlayTone(TONE_B7, 5); fileHeader = “strgyroRaw, strgOffset, strgyroSpeed , strgyroAngle , strtInterval, A0raw, A1raw”; header for myData file WriteLnString(fileHandle, fileHeader, bytesWritten); NXT Guide Section 6.59.2.43 pg. 554 Write string and new line to a file bytesWritten is an unsigned int. Its value is # of bytes written } end InitWriteToFile void WriteToFile(string strTempText) { strTempText stores the text (i.e. ticks and motorRpm to be written to file write string to file result=WriteLnString(fileHandle, strTempText, bytesWritten); if the end of file is reached, close the file and create a new part if (result==LDR_EOFEXPECTED) LDR_EOFEXPECTED is flagged when end-of-file { close the current file CloseFile(fileHandle); NXT Guide Section 6.59.2.1 pg. 535 Closes file associated with file handle create the next file name filePart = filePart + 1; strFileNumber = NumToStr(fileNumber); strFilePart = NumToStr(filePart); fileName = StrCat(“myMotorSpeed”, strFileNumber,“-”, strFilePart ,“.csv”); delete the file if it exists DeleteFile(fileName); NXT Guide Section 6.59.2.5 pg. 537 Delete the file specified by the string input create a new file CreateFile(fileName, filesize, fileHandle); play a tone every time a file is created PlayTone(TONE_B7, 5); WriteLnString(fileHandle, strTempText, bytesWritten); } end if } end WriteToFile Close the file void StopWriteToFile() { close the file CloseFile(fileHandle); } end StopWriteToFile void SetWheelRatio() This function implements a UI so that the user can choose the size of the wheels used on the robot. void SetWheelRatio() { int nSelect; Start of with Medium (NXT 1.0) selected nSelect = 2; do { Display base text of user interface on screen ClearScreen(); TextOut(0, LCD_LINE1, “HiTechnic-HTWayC”); TextOut(6, LCD_LINE2, “Set wheel size:”); TextOut(2, LCD_LINE8, “< [Select] >”); Display current selection switch (nSelect) { case 1: Small CircleOut(50, 32, 6); TextOut(7, LCD_LINE7, “Small (NXT 2.0)”); ratioWheel = WHEEL_RATIO_NXT2; break; case 2: Medium CircleOut(50, 32, 10); TextOut(2, LCD_LINE7, “Medium (NXT 1.0)”); ratioWheel = WHEEL_RATIO_NXT1; break; case 3: Large CircleOut(50, 32, 14); TextOut(17, LCD_LINE7, “Large (RCX)”); ratioWheel = WHEEL_RATIO_RCX; break; } Make sure previous button is released, loop until all released while(ButtonPressed(BTNLEFT, false) || ButtonPressed(BTNCENTER, false) || ButtonPressed(BTNRIGHT, false)); Get button press while(true) { if (ButtonPressed(BTNLEFT, false)) { nSelect–; if (nSelect < 1) nSelect = 3; break; } if (ButtonPressed(BTNRIGHT, false)) { nSelect++; if (nSelect > 3) nSelect = 1; break; } if (ButtonPressed(BTNCENTER, false)) break; } } until (ButtonPressed(BTNCENTER, false)); ClearScreen(); } ===================================================================== GetGyroOffset This function returns a suitable initial gyro offset. It takes 100 gyro samples over a time of 1/2 second and averages them to get the offset. It also check the max and min during that time and if the difference is larger than one it rejects the data and gets another set of samples. #define OFFSET_SAMPLES 100 void GetGyroOffset() { float gSum; int i, gMin, gMax, g; ClearScreen(); TextOut(0, LCD_LINE1, “HiTechnic-HTWayC”); TextOut(0, LCD_LINE4, “Lay robot down”); TextOut(0, LCD_LINE5, “flat to get gyro”); TextOut(0, LCD_LINE6, “offset.”); Ensure that the motor controller is active since this affects the gyro values. Off(MOTORS); do { gSum = 0.0; gMin = 1000; gMax = -1000; for (i=0; i<OFFSET_SAMPLES; i++) { readdataA0(); g = (A0raw-384.524)*(-0.98); if (g > gMax) gMax = g; if (g < gMin) gMin = g; gSum += g; Wait(5); } } while ((gMax - gMin) > 1); Reject and sample again if range too large PlayTone(440,1000); Average the sum of the samples. gOffset = gSum / OFFSET_SAMPLES + 1.0; Even with motor controller active, the initial offset appears to be off from the actual needed offset to keep robot from wondering. This +1 helps keep robot from wondering when it first starts to balance. } ===================================================================== void StartBeeps() { int c; ClearScreen(); TextOut(0, LCD_LINE1, “HiTechnic-HTWayC”); TextOut(20, LCD_LINE3, “Balance in”); Play warning beep sequence to indicate balance about to start for (c=5; c>0;c–) { NumOut(47, LCD_LINE4, c); PlayTone(440,100); Wait(1000); } NumOut(47, LCD_LINE4, 0); PlayTone(440,1000); Wait(1000); } ===================================================================== task main - initializes the sensors, Both taskBalance and taskController start when main ends. task main() { InitWriteToFile(); Initialize Proto Board SetSensorLowspeed(PROTO_PORT); set sensor port 1 to low speed serial (I2C) Wait(100); ArrayInit(cmndbuf, 0, 3); set the buffer to hold 3 values cmndbuf[0] = 0x02; set write to channel cmndbuf[1] = 0x4E; to set write address cmndbuf[2] = 0x3F; to write 111111 count=0; no bytes to read I2CBytes(PROTO_PORT, cmndbuf, count, respbuf); issue I2C write command Wait(100); Initialize the sensors SetSensorHTGyro(GYRO); SetSensorLowspeed(IR_RECEIVER); Wait(50); Get user input on wheel size SetWheelRatio(); Get the initial gyro offset GetGyroOffset(); Play warning beep sequence before balance starts StartBeeps(); When task main ends, both taskBalance and taskControl will start } ===================================================================== GetGyroData(float &gyroSpeed, float &gyroAngle) Get the data from the gyro. Fills the pass by reference gyroSpeed and gyroAngle based on updated information from the Gyro Sensor Maintains an automatically adjusted gyro offset as well as the integrated gyro angle. inline void GetGyroData(float &gyroSpeed, float &gyroAngle) { float gyroRaw; gyroRaw = SensorHTGyro(GYRO); readdataA0(); gyroRaw = (A0raw-384.524)*(-0.98); readdataA1(); NumOut(20, LCD_LINE5, gyroRaw); ClearScreen(); gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset; gyroSpeed = gyroRaw - gOffset; NumOut(20, LCD_LINE6, gyroSpeed); NumOut(20, LCD_LINE7, A0raw); gAngleGlobal += gyroSpeed*tInterval; gyroAngle = gAngleGlobal; strgyroRaw = FormatNum(“%5.6f”, gyroRaw); strgOffset = FormatNum(“%5.6f”, gOffset); strgyroSpeed = FormatNum(“%5.6f”, gyroSpeed); strgyroAngle = FormatNum(“%5.6f”, gyroAngle); strtInterval = FormatNum(“%5.6f”, tInterval); strA0raw = FormatNum(“%5.6f”, A0raw); strA1raw = FormatNum(“%5.6f”, A1raw); text=StrCat(strgyroRaw,“,”, strA0raw); text=StrCat(strgyroRaw, “,”, strgOffset,“,”, strgyroSpeed, “,”, strgyroAngle,“,”, strtInterval, “,”, strA0raw); “,”, strA1raw ); WriteToFile(text); } ——————————————————————— inline void GetMotorData(float &motorSpeed, float &motorPos) { long mrcLeft, mrcRight, mrcDelta; Keep track of motor position and speed mrcLeft = MotorRotationCount(LEFT_MOTOR); mrcRight = MotorRotationCount(RIGHT_MOTOR); Maintain previus mrcSum so that delta can be calculated and get new mrcSum and Diff values mrcSumPrev = mrcSum; mrcSum = mrcLeft + mrcRight; motorDiff = mrcLeft - mrcRight; mrcDetla is the change int sum of the motor encoders, update motorPos based on this detla mrcDelta = mrcSum - mrcSumPrev; motorPos += mrcDelta; motorSpeed is based on the average of the last four delta's. motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval); Shift the latest mrcDelta into the previous three saved delta values mrcDeltaP3 = mrcDeltaP2; mrcDeltaP2 = mrcDeltaP1; mrcDeltaP1 = mrcDelta; } ——————————————————————— void SteerControl(int power, int &powerLeft, int &powerRight) This function determines the left and right motor power that should be used based on the balance power and the steering control inline void SteerControl(int power, int &powerLeft, int &powerRight) { int powerSteer; Update the target motor difference based on the user steering control value. motorDiffTarget += motorControlSteer * tInterval; Determine the proportionate power differential to be used based on the difference between the target motor difference and the actual motor difference. powerSteer = KSTEER * (motorDiffTarget - motorDiff); Apply the power steering value with the main power value to get the left and right power values. powerLeft = power + powerSteer; powerRight = power - powerSteer; Limit the power to motor power range -100 to 100 if (powerLeft > 100) powerLeft = 100; if (powerLeft < -100) powerLeft = -100; Limit the power to motor power range -100 to 100 if (powerRight > 100) powerRight = 100; if (powerRight < -100) powerRight = -100; } ——————————————————————— void CalcInterval(long cLoop) Calculate the interval time from one iteration of the loop to the next. Note that first time through, cLoop is 0, and has not gone through the body of the loop yet. Use it to save the start time. After the first iteration, take the average time and convert it to seconds for use as interval time. inline void CalcInterval(long cLoop) { if (cLoop == 0) { First time through, set an initial tInterval time and record start time tInterval = 0.0055; tCalcStart = CurrentTick(); } else { Take average of number of times through the loop and use for interval time. tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000.0); } } ——————————————————————— taskBalance This is the main balance task for the HTWay robot. Robot is assumed to start leaning on a wall. The first thing it does is take multiple samples of the gyro sensor to establish and initial gyro offset. After an initial gyro offset is established, the robot backs up against the wall until it falls forward, when it detects the forward fall, it start the balance loop. The main state variables are: gyroAngle This is the angle of the robot, it is the results of integrating on the gyro value. Units: degrees gyroSpeed The value from the Gyro Sensor after offset subtracted Units: degrees/second motorPos This is the motor position used for balancing. Note that this variable has two sources of input: Change in motor position based on the sum of MotorRotationCount of the two motors, and, forced movement based on user driving the robot. Units: degrees (sum of the two motors) motorSpeed This is the speed of the wheels of the robot based on the motor encoders. Units: degrees/second (sum of the two motors) From these state variables, the power to the motors is determined by this linear equation: power = KGYROSPEED * gyro + KGYROANGLE * gyroAngle + KPOS * motorPos + KSPEED * motorSpeed; task taskBalance() { Follows(main); float gyroSpeed, gyroAngle; float motorSpeed; int power, powerLeft, powerRight; long tMotorPosOK; long cLoop = 0; ClearScreen(); TextOut(0, LCD_LINE1, “HiTechnic-HTWayC”); TextOut(0, LCD_LINE4, “Balancing”); tMotorPosOK = CurrentTick(); Reset the motors to make sure we start at a zero position ResetRotationCount(LEFT_MOTOR); ResetRotationCount(RIGHT_MOTOR); while(true) { CalcInterval(cLoop++); GetGyroData(gyroSpeed, gyroAngle); GetMotorData(motorSpeed, motorPos); Apply the drive control value to the motor position to get robot to move. motorPos -= motorControlDrive * tInterval; This is the main balancing equation power = (KGYROSPEED * gyroSpeed + Deg/Sec from Gyro sensor KGYROANGLE * gyroAngle) / ratioWheel + Deg from integral of gyro KPOS * motorPos + From MotorRotaionCount of both motors KDRIVE * motorControlDrive + To improve start/stop performance KSPEED * motorSpeed; Motor speed in Deg/Sec if (abs(power) < 100) tMotorPosOK = CurrentTick(); SteerControl(power, powerLeft, powerRight); Apply the power values to the motors OnFwd(LEFT_MOTOR, powerLeft); OnFwd(RIGHT_MOTOR, powerRight); Check if robot has fallen by detecting that motorPos is being limited for an extended amount of time. if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) { break; } Wait(WAIT_TIME); } Off(MOTORS); TextOut(0, LCD_LINE4, “Oops… I fell”); TextOut(0, LCD_LINE8, “tInt ms:”); NumOut(6*8, LCD_LINE8, tInterval*1000); } ===================================================================== taskControl This task runs in parallel to taskBalance. This task monitors the IR Receiver and sets the global variables motorControlDrive and motorControlSteer. Both of these values are in degrees/second. task taskControl() { Follows(main); char pfData[8]; while(true) { ReadSensorHTIRReceiver(IR_RECEIVER, pfData); if (pfData[IR_LEFT] == -128) pfData[IR_LEFT] = 0; if (pfData[IR_RIGHT] == -128) pfData[IR_RIGHT] = 0; Set control Drive and Steer. These are in motor degree/second motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0; motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0; Wait(CONTROL_WAIT); } Wait to allow user time to see screen. Wait(10000); } </code> nxtway_justimu.nxc.zip