Hi, I’m from team 2856 (Sector SAAS) and we’ve been having some really weird problems with our teleop. Our robot has 2 lifters that grab and raise a crate. The lifters are attached to the robot via chains and a motor on each side of the robot. Often times these two lifters run at vastly different speeds despite being set to the same power value. Each motor has encoders attached for bounds, and we are using the built-in RobotC PID for speed control. We have “compensated” for the speed difference by modifying the teleop so that one motor runs at 5% power and the other at 60% power! At the same time that we see this phenomenon, our “turbo” and “slow” parts of our tank drive don’t work (that is all under tankdrive(), see below).
The phenomena is buggy and nonpersistent; at times we see it, and at other times the teleop works perfectly with the turbo and slow functioning, and the lifters going at the vastly different speeds which would make sense for powers of 5 and 60.
We are using RobotC 3.04, but we also see these bugs on another laptop running RobotC 3.01. I am wondering if this is a version/firmware problem, an issue with DC motor controller, or if it is a problem with our program. We are also using an altered joystickdriver.

Main code:
#pragma config(Hubs, S1, HTServo, HTMotor, HTMotor, HTMotor)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C2_1, Ldrive, tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_2, CrateLiftL, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C3_1, BallVac, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_2, CrateLiftR, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C4_1, motorH, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C4_2, Rdrive, tmotorNormal, PIDControl, encoder)
#pragma config(Servo, srvo_S1_C1_1, LeftGate, tServoStandard)
#pragma config(Servo, srvo_S1_C1_2, RightGate, tServoStandard)
#pragma config(Servo, srvo_S1_C1_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C1_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C1_5, RBC, tServoStandard)
//#pragma config(Servo, srvo_S1_C1_6, RFC, tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

#include "C:\Users\Public\Documents\Robotics\Includes\JoystickDriver.h"
#include "auxiliaryfunctions.h"

int VacState = -1;
int VacSpeed = 0;

void initialize()
{
//servo[LeftGate] = 60;//good
//servo[RightGate] = 175;//good
servo[RBC] = 108;
nMotorEncoder[CrateLiftL] = 0;
nMotorEncoder[CrateLiftR] = 0;

}

//////////////////////////////////////////////////////////////
void ballLifter()
{
if (joyBtnOnce(2, 2) == 1)
{
VacState = -VacState;
}
if (VacState == -1) // when not pressed
{
if(wait(50,0))
{
bFloatDuringInactiveMotorPWM = true;
VacSpeed=VacSpeed + 5; // increment speed down every 50 ms
}
if (VacSpeed > 0)
{
VacSpeed = 0;
bFloatDuringInactiveMotorPWM = false;
}
motor[BallVac] = VacSpeed;
servo[LeftGate] = 60;//good
servo[RightGate] = 175;//good
}
else // when pressed
{
VacSpeed = -80;
motor[BallVac] = VacSpeed;
servo[LeftGate] = 150; //good
servo[RightGate] = 86;//good
}
}

void tankDrive()
{
// integer setup:
int speed;
int button = 0;


// setup for the speed structure:
if (joy1Btn(6) == 1)
{
button = 1;
}
if (joy1Btn(5) == 1)
{
button = 2;
}

switch (button)
{
// write "speed" based on the above cases
case 1:
speed = 100;
break;

case 2:
speed = 15;
break;

default:
speed = 40;
break;
}



// simple tank drive
if (abs (joyY(1,0)) > 15)
{

motor[Ldrive] = joyY(1,0) * speed / 128; // scaling the power
}
else
{
motor[Ldrive] = 0;
}

if (abs (joyY(1,1)) > 15)
{

motor[Rdrive] = joyY(1,1) * speed / 128;
}
else
{
motor[Rdrive] = 0;
}
return;
}


//////////////////////////////////////////
void CrateLifter()
{
nxtDisplayString(7, "CrateL %d", nMotorEncoder[CrateLiftL]);
nxtDisplayString(6, "CrateR %d", nMotorEncoder[CrateLiftR]);
if(joy2Btn(6) == 1 && nMotorEncoder[CrateLiftR] > -2475)
{
motor[CrateLiftR] = -20;
}
else if(joy2Btn(8) == 1 && nMotorEncoder[CrateLiftR] < 0)
{
motor[CrateLiftR] = 20;
}
else
{
motor[CrateLiftR] = 0;
}


if(joy2Btn(5) == 1 && nMotorEncoder[CrateLiftL] <= 2500)
{
motor[CrateLiftL] = -18;
}
else if(joy2Btn(7) == 1 && nMotorEncoder[CrateLiftL] >= 0)
{
motor[CrateLiftL] = 18;
}
else
{
motor[CrateLiftL] = 0;
}

if (joy2Btn(3) == 1)
{
servo[RFC] = 200;
servo[RBC] = 55;
}
else
{
servo[RFC] = 108;
servo[RBC] = 148;
}
}



task main()
{
getJoystickSettings(joystick);
initialize();
while (true)
{

if(getJoystickSettings(joystick) == 1)
{

if (joy1Btn(8) == 0)
{
tankDrive();
}
else
{
motor[Rdrive] = 0;
motor[Ldrive] = 0;
}
CrateLifter();
ballLifter();

}
}
}








JoystickDriver.h:

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// JoystickDriver.h
//
// A drop-in-compatible replacment for JoystickDriver.c, with enhancements and fixes. Brought
// to you by FTC Team 417.
//
// With the TETRIX system, the PC Controller Station sends messages over Bluetooth or Samantha to the NXT
// containing current settings of a PC joystick. The joystick settings arrive using the NXT
// "message mailbox" facility.
//
// End users almost never need to modify this file; rather, one simply #includes it, periodically
// calls getJoystickSettings(joystick), and then uses joyBtn(), joyHat(), joyX(), joyY() etc to
// examine the joystick state.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if defined(NXT) || defined(TETRIX)
#pragma autoStartTasks // Automatically start tasks (which ones?) when the main user program starts.
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Compability stuff for when this file stands alone
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef BOOL
#define BOOL bool
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Constants: some reasonable names for joystick-related buttons etc
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

// Easy-to-remember names for the various buttons on a joystick controller
typedef enum
{
JOYBTN_FIRST =1,
JOYBTN_1 =JOYBTN_FIRST,
JOYBTN_2 =2,
JOYBTN_3 =3,
JOYBTN_4 =4,
JOYBTN_LEFTTRIGGER_UPPER =5,
JOYBTN_RIGHTTRIGGER_UPPER =6,
JOYBTN_LEFTTRIGGER_LOWER =7,
JOYBTN_RIGHTTRIGGER_LOWER =8,
JOYBTN_TOP_LEFT =9,
JOYBTN_TOP_RIGHT =10,
JOYBTN_JOYSTICK_LEFT =11,
JOYBTN_JOYSTICK_RIGHT =12,
JOYBTN_LAST =JOYBTN_JOYSTICK_RIGHT,
} JOYBTN;

// Easy-to-remember names for the eight directions that the hat on a controller may be pushed
typedef enum
{
JOYHAT_NONE = -1, // hat not pressed
JOYHAT_FIRST = 0,
JOYHAT_UP = JOYHAT_FIRST, // hat pressed up
JOYHAT_UPRIGHT = 1, // hat pressed up and to the right
JOYHAT_RIGHT = 2, // etc...
JOYHAT_DOWNRIGHT = 3,
JOYHAT_DOWN = 4,
JOYHAT_DOWNLEFT = 5,
JOYHAT_LEFT = 6,
JOYHAT_UPLEFT = 7,
JOYHAT_LAST = JOYHAT_UPLEFT,
} JOYHAT;

// Easy-to-remember names for the two joysticks on each joystick controller
typedef enum
{
JOY_LEFT=0,
JOY_RIGHT=1,
} IJOY;

// Names for the directions in which a joystick may be flicked. Note that
// that these may be 'or'd together, as in the following example:
//
// if (joyFlickOnce(1,JOY_LEFT,JOYDIR_UP|JOYDIR_DOWN))
// {
// // Here do stuff common to both up and down
// if (joyFlick(1,JOY_LEFT,JOYDIR_UP))
// {
// // Here do up stuff
// }
// else
// {
// // Here do down stuff
// }
// }
//
typedef enum
{
JOYDIR_UP = 1,
JOYDIR_DOWN = 2,
JOYDIR_LEFT = 4,
JOYDIR_RIGHT = 8,
} JOYDIR;

// Value for size of the the neutral region of a joystick within which the
// joystick is to be considered not to have moved.
int joyFlickDeadZone = 45;
int joyThrottleDeadZone = 15;

#ifndef Max
#define Max(a,b) ((a) > (b) ? (a) : (b))
#endif

#ifndef Min
#define Min(a,b) ((a) < (b) ? (a) : (b))
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Structure declarations
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

// State of one joystick (there are two of these per controller)
typedef struct
{
short x; // -128 to +127
short y; // -128 to +127
} JOYMSG;

// State of one joystick controller as messaged from the PC
typedef struct
{
JOYMSG rgjoy[2];
short buttons; // bitmap of values computed from JOYBTN
short hat; // values are from JOYHAT
} JOYCNTMSG;

// Auxiliary state we keep for each controller beyond that sent from the PC
typedef struct
{
short buttonsOnce; // Bitmap for managing single button presses
short hatOnce; // Bitmap for managing single hat presses
short rgflicks[2]; // Bit map for supporting 'flicks' with the two joysticks in this controller
} JOYCNTAUX;

// State of all the joystick controllers as updated from messages from the PC
typedef struct
{
BOOL fTeleOp; // autonomous(false) vs tele-operated(true) mode.
BOOL fWaitForStart; // becomes false when the FTC Field Control System permits the program to proceed.
long serialNumber; // # of current msg used to set this state. Used to prevent replay of messages.
long msReceived; // time on the system clock (nSysTime) at which this message was received. Only updated when serialNumber is incremented.
JOYCNTMSG rgcnt[2];
} JOYSTICKSMSG;

// State of all the joystick controllers, both the state from the PC and the once-management state
typedef struct
{
JOYSTICKSMSG msg; // as updated from the PC. This MUST come first in TJoystick
JOYCNTAUX rgaux[2]; // auxiliary state we maintain for each controller beyond that sent from the PC
} TJoystick;

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// User-callable joystick functions and macros.
//
// Call getJoystickSettings() to read the latest values from the joystick. It returns true if there's
// new data available to process since the last call; false otherwise.
//
// joyBtn() will return true or false accoring to the current state of the requested button; if the button
// is held, true is continually returned. By contrast, joyBtnOnce() will return true immediately after the
// button is pressed but will not again return true until the button is released and then pressed again.
//
// joyHat() and joyHatOnce() behave similarly
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

// The typical/usual variable used as the parameter for getJoystickSettings
TJoystick joystick;

// Update the joystick state into the indicated variable (almost always 'joystick'). Answer whether
// there's any new information about the joystick since the last time getJoystickSettings was called: if
// 'false' is returned, you probably should avoid processing the joystick data as you likely did previously
// when 'true' was returned.
BOOL getJoystickSettings(TJoystick& joystickVar);

// Manually update the state of this joystick varaible to the 'nothing pressed state'.
// Handy, but not perhaps as much as one might think.
#define ResetJoystickToNothingPressed(joystickVar) \
{ \
hogCpu(); /* avoid race with readMsgFromPC task */ \
memset(joystickVar.msg.rgcnt[0], 0, sizeof(JOYCNTMSG)); \
memset(joystickVar.msg.rgcnt[1], 0, sizeof(JOYCNTMSG)); \
memset(joystickVar.rgaux[0], 0, sizeof(JOYCNTAUX)); \
memset(joystickVar.rgaux[1], 0, sizeof(JOYCNTAUX)); \
joystickVar.msg.rgcnt[0].hat = JOYHAT_NONE; \
joystickVar.msg.rgcnt[1].hat = JOYHAT_NONE; \
releaseCpu(); \
}

// What's a reasonable length of time beyond which we believe that the Field Control System
// has just gone away.
#ifndef MS_JOYSTICK_FCS_DISCONNECTED_THRESHOLD
#define MS_JOYSTICK_FCS_DISCONNECTED_THRESHOLD 1000 // a reasonable value, but not exact by any means
#endif

// 'Functions' for accessing the various state of the two joystick controllers. Note that these *assume*
// that getJoystickSettings() was called with the variable 'joystick' as its parameter.
#define joyBtn(jyc,btn) ((_joyBtnState(jyc) & _joyBtnBit(btn)) != 0)
#define joyHat(jyc,hatVal) (joystick.msg.rgcnt[jyc-1].hat==(hatVal))
#define joyX(jyc,ijoy) (joystick.msg.rgcnt[jyc-1].rgjoy[ijoy].x)
#define joyY(jyc,ijoy) (joystick.msg.rgcnt[jyc-1].rgjoy[ijoy].y)

// Functions that test button and hat state but only return 'true' once per press
BOOL joyBtnOnce(int jyc, int btn);
BOOL joyHatOnce(int jyc, int hat);

// 'Flick'ing a joystick allows the four directions on each joystick (on each controller) to be used
// very much like additional controller buttons (instead of, say, a throttle as is typically done
// when manually driving).
//
// A 'flick once' will fire only once; it will not refire until the joystick is returned to the neutral
// dead zone (this is similar to joyBtnOnce()). If instead you wish to repeatedly fire so long as
// the joystick remains displaced, use joyFlick().
BOOL joyFlickOnce(int jyc, int ijoy, int flick);
BOOL joyFlick(int jyc, int ijoy, int flick);

// When was the last joystick message received?
#define joyMessageTime() joystick.msg.msReceived

// How many joystick messages have we seen?
#define joyMessageCount() joystick.msg.serialNumber

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Legacy support - compatibility definitions - deprecated; to be eliminated over time.
// Try to avoid using these in new code - use joyBtn(), joyX(), etc instead (see above).
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define joy1_x1 msg.rgcnt[0].rgjoy[JOY_LEFT].x
#define joy1_y1 msg.rgcnt[0].rgjoy[JOY_LEFT].y
#define joy1_x2 msg.rgcnt[0].rgjoy[JOY_RIGHT].x
#define joy1_y2 msg.rgcnt[0].rgjoy[JOY_RIGHT].y
#define joy1_Buttons msg.rgcnt[0].buttons
#define joy1_TopHat msg.rgcnt[0].hat

#define joy2_x1 msg.rgcnt[1].rgjoy[JOY_LEFT].x
#define joy2_y1 msg.rgcnt[1].rgjoy[JOY_LEFT].y
#define joy2_x2 msg.rgcnt[1].rgjoy[JOY_RIGHT].x
#define joy2_y2 msg.rgcnt[1].rgjoy[JOY_RIGHT].y
#define joy2_Buttons msg.rgcnt[1].buttons
#define joy2_TopHat msg.rgcnt[1].hat

#define joy1Btn(btn) ((joystick.msg.rgcnt[0].buttons & _joyBtnBit(btn)) != 0)
#define joy2Btn(btn) ((joystick.msg.rgcnt[1].buttons & _joyBtnBit(btn)) != 0)

#define joyLeftX(jyc) joyX(jyc,JOY_LEFT)
#define joyLeftY(jyc) joyY(jyc,JOY_LEFT)
#define joyRightX(jyc) joyX(jyc,JOY_RIGHT)
#define joyRightY(jyc) joyY(jyc,JOY_RIGHT)


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Internals
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define _joyBtnOnce_(jyc,btn) \
(joyBtn(jyc,btn) \
? ((_joyBtnOnceState(jyc) & _joyBtnBit(btn)) != 0 \
? false \
: _trueOf(_joyBtnOnceState(jyc) |= _joyBtnBit(btn))) \
: _falseOf(_joyBtnOnceState(jyc) &= ~_joyBtnBit(btn)))

#define _joyHatOnce_(jyc,hat) \
((joyHat(jyc,hat)) \
? ((_joyHatOnceState(jyc) & _joyHatBit(hat)) != 0 \
? false \
: _trueOf(_joyHatOnceState(jyc) |= _joyHatBit(hat))) \
: _falseOf(_joyHatOnceState(jyc) &= ~_joyHatBit(hat)))

#define _joyFlick_(jyc,ijoy,flick) \
(((flick) & JOYDIR_UP) && (joyY(jyc,ijoy) > joyFlickDeadZone) \
? true \
: (((flick) & JOYDIR_DOWN) && (joyY(jyc,ijoy) < -joyFlickDeadZone) \
? true \
: (((flick) & JOYDIR_RIGHT) && (joyX(jyc,ijoy) > joyFlickDeadZone) \
? true \
: (((flick) & JOYDIR_LEFT) && (joyX(jyc,ijoy) < -joyFlickDeadZone) \
? true \
: false))))

#define _joyFlickOnce_(jyc,ijoy,flick) \
(_joyFlick_(jyc,ijoy,flick) \
? ((_joyFlickState(jyc,ijoy) & (flick)) != 0 \
? false \
: _trueOf(_joyFlickState(jyc,ijoy) |= (flick))) \
: _falseOf(_joyFlickState(jyc,ijoy) &= ~(flick)))

// The PC delivers joystick messages to a particular mailbox address in the NXT firmware, namely this one
const TMailboxIDs kJoystickQueueID = mailbox1;

// Internal buffer to hold the last received message from the PC (do not directly use)
JOYSTICKSMSG _joystickInternal;

// Copy the msg portion of the structure from the internal messaging state.
// NB: memcpy, being one opcode, is atomic
#define _getJoystickSettingsPrim(joystickVar) memcpy(joystickVar, _joystickInternal, sizeof(_joystickInternal))

// Update the joystick state. Answer whether there's anything new there to process.
BOOL getJoystickSettings(TJoystick& joystickVar)
{
long serialNumberPrev = joystickVar.msg.serialNumber;
_getJoystickSettingsPrim(joystickVar);
return joystickVar.msg.serialNumber != serialNumberPrev;
}

#define _joyBtnBit(btn) (1 << (btn - 1))
#define _joyHatBit(hat) (1 << (hat)) // 'hat' is from JOY_HAT. NB: don't use with '-1'
#define _joyBtnState(jyc) joystick.msg.rgcnt[jyc-1].buttons
#define _joyHatState(jyc) joystick.msg.rgcnt[jyc-1].hat

#define _joyBtnOnceState(jyc) joystick.rgaux[jyc-1].buttonsOnce
#define _joyHatOnceState(jyc) joystick.rgaux[jyc-1].hatOnce
#define _joyFlickState(jyc,ijoy) joystick.rgaux[jyc-1].rgflicks[ijoy]

#define _trueOf(expr) ((expr) || 1)
#define _falseOf(expr) ((expr) && 0)

BOOL joyBtnOnce(int jyc, int btn)
{
return _joyBtnOnce_(jyc, btn);
}
BOOL joyHatOnce(int jyc, int hat)
{
return _joyHatOnce_(jyc, hat);
}
BOOL joyFlickOnce(int jyc, int ijoy, int flick)
{
return _joyFlickOnce_(jyc, ijoy, flick);
}
BOOL joyFlick(int jyc, int ijoy, int flick)
{
return _joyFlick_(jyc, ijoy, flick);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Receive Messages Task
//
// Dedicated task that continuously polls for a Bluetooth/Samantha message from the PC containing the joystick
// values. Operation of this task is nearly transparent to the end user as the earlier "#pragma autoStartTasks"
// statement above will cause it to start running as soon as the user program is started.
//
// The task is very simple. It's an endless loop that continuously looks for a mailbox
// message from the PC. When one is found, it reformats and copies the contents into the internal
// "_joystickInternal" buffer.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#define InitializeJoystickGlobals() \
{ \
memset(_joystickInternal, 0, sizeof(_joystickInternal)); \
_joystickInternal.fWaitForStart = true; \
_joystickInternal.rgcnt[0].hat = -1; \
_joystickInternal.rgcnt[1].hat = -1; \
}

task readMsgFromPC()
{
// Initialize setting to default values in case communications with PC is broken.
InitializeJoystickGlobals();

while (true)
{
const int kMaxSizeOfMessage = 18;
sbyte rgbT[kMaxSizeOfMessage];

// Check to see if a message is available.
BOOL fMsgFound = false;
while (true)
{
// There may be more than one message in the queue. We want to get to the last received
// message and discard the earlier "stale" messages. This loop simply discards all but
// the last message.
//
int cbMessage = cCmdMessageGetSize(kJoystickQueueID);
if (cbMessage <= 0)
{
if (!fMsgFound)
{
wait1Msec(4); // Give other tasks a chance to run
continue; // No message this time. Loop again
}
//
// No more messages available and at least one message found. We simply discard earlier
// messages: each joystick message is self contained, representing the entire state of
// of the joysticks, so the earlier ones have no use.
//
break;
}

// OK: there's at least one message there; read it!
if (cbMessage > sizeof(rgbT))
cbMessage = sizeof(rgbT);

// cCmdMessageRead returns
// ioRsltSuccess on success
// ioRsltEmptyMailbox if the message queue is empty (but we call cCmdMessageGetSize first above)
// ERR_INVALID_SIZE if the buffer passed here is too small
// a few others
TFileIOResult nBTCmdRdErrorStatus = cCmdMessageRead((ubyte)rgbT, cbMessage, kJoystickQueueID);
if (ioRsltSuccess == nBTCmdRdErrorStatus)
{
// Repeat loop until there are no more messages in the queue. We only want to process the
// last message in the queue.
fMsgFound = true;
}
}

hogCPU(); // grab CPU for duration of critical section

_joystickInternal.fTeleOp = rgbT[1];
_joystickInternal.fWaitForStart = rgbT[2];

_joystickInternal.rgcnt[0].rgjoy[JOY_LEFT].x = rgbT[3];
_joystickInternal.rgcnt[0].rgjoy[JOY_LEFT].y = rgbT[4];
_joystickInternal.rgcnt[0].rgjoy[JOY_RIGHT].x = rgbT[5];
_joystickInternal.rgcnt[0].rgjoy[JOY_RIGHT].y = rgbT[6];
_joystickInternal.rgcnt[0].buttons = (rgbT[7] & 0x00FF) | (rgbT[8] << ;
_joystickInternal.rgcnt[0].hat = rgbT[9];

_joystickInternal.rgcnt[1].rgjoy[JOY_LEFT].x = rgbT[10];
_joystickInternal.rgcnt[1].rgjoy[JOY_LEFT].y = rgbT[11];
_joystickInternal.rgcnt[1].rgjoy[JOY_RIGHT].x = rgbT[12];
_joystickInternal.rgcnt[1].rgjoy[JOY_RIGHT].y = rgbT[13];
_joystickInternal.rgcnt[1].buttons = (rgbT[14] & 0x00FF) | (rgbT[15] << ;
_joystickInternal.rgcnt[1].hat = rgbT[16];

// If control is started with *no* joysticks attached (or at least none logically connected
// to RobotC) then the message that arrives from the PC has *entirely* zero values for all joysticks.
// Unfortunately, and annoyingly, in that condition, the hat is logically JOYHAT_UP, which
// will be interpreted by many programs as the hat being actively pushed, which will cause
// some action to be taken by the program, almost certainly in error. As a work around, we
// refuse to process any received messages until we see at least one with the first joystick's
// hat not seemingly in the 'up' position. So: hands off the hat at the start of your program!
if ((_joystickInternal.serialNumber != 0) || (_joystickInternal.rgcnt[0].hat != 0))
{
_joystickInternal.serialNumber++;
_joystickInternal.msReceived = nSysTime;
}

_joystickInternal.rgcnt[0].rgjoy[JOY_LEFT].y = -_joystickInternal.rgcnt[0].rgjoy[JOY_LEFT].y; // Negate to "natural" position
_joystickInternal.rgcnt[0].rgjoy[JOY_RIGHT].y = -_joystickInternal.rgcnt[0].rgjoy[JOY_RIGHT].y; // Negate to "natural" position

_joystickInternal.rgcnt[1].rgjoy[JOY_LEFT].y = -_joystickInternal.rgcnt[1].rgjoy[JOY_LEFT].y; // Negate to "natural" position
_joystickInternal.rgcnt[1].rgjoy[JOY_RIGHT].y = -_joystickInternal.rgcnt[1].rgjoy[JOY_RIGHT].y; // Negate to "natural" position

releaseCPU(); // end of critical section
}
}

#if defined(TETRIX)

///////////////////////////////////////////////////////////////////////////////////////////
//
// getUserControlProgram
//
// This function returns the name of the TETRIX User Control program. It reads the file
// "FTCConfig.txt" and builds the name of the file from the contents.
//
// Note that the filename includes the ".rxe" (robot executable file) file extension.
//
///////////////////////////////////////////////////////////////////////////////////////////

const string kConfigName = "FTCConfig.txt";

#define getUserControlProgram(szFileName) \
{ \
szFileName = ""; \
\
byte rgb[2]; \
rgb[1] = 0; \
\
int cbFile; \
TFileIOResult nIoResult; \
TFileHandle hFileHandle = 0; \
\
OpenRead(hFileHandle, nIoResult, kConfigName, cbFile); \
if (nIoResult == ioRsltSuccess) \
{ \
for (int ib = 0; ib < cbFile; ++ib) \
{ \
ReadByte(hFileHandle, nIoResult,rgb[0]); \
strcat(szFileName, rgb); \
} \
\
/* Delete the ".rxe" file extension */ \
int ichFileExt = strlen(szFileName) - 4; \
if (ichFileExt > 0) \
StringDelete(szFileName, ichFileExt, 4); \
} \
Close(hFileHandle, nIoResult); \
}

///////////////////////////////////////////////////////////////////////////////////////////
//
// displayDiagnostics
//
// THis task will display diagnostic information about a TETRIX robot on the NXT LCD.
//
// If you want to use the LCD for your own debugging use, call the function
// "disableDiagnosticsDisplay()
//
///////////////////////////////////////////////////////////////////////////////////////////

#ifndef USE_DISPLAY_DIAGNOSTICS
#define USE_DISPLAY_DIAGNOSTICS 1
#endif

#if USE_DISPLAY_DIAGNOSTICS
BOOL bDisplayDiagnostics = true; // Set to false in user program to disable diagnostic display

#define disableDiagnosticsDisplay() \
{ \
bDisplayDiagnostics = false; \
}

task displayDiagnostics()
{
string szFileName;
getUserControlProgram(szFileName);

nxtDisplayTextLine(6, "Teleop FileName:");
nxtDisplayTextLine(7, szFileName);
bNxtLCDStatusDisplay = true;
while (bDisplayDiagnostics)
{
if (bDisplayDiagnostics)
{
_getJoystickSettingsPrim(joystick); // Update variables with current joystick values

if (joystick.msg.fWaitForStart)
nxtDisplayCenteredTextLine(1, "Wait for Start");
else if (joystick.msg.fTeleOp)
nxtDisplayCenteredTextLine(1, "TeleOp Running");
else
nxtDisplayCenteredTextLine(1, "Auton Running");

if ( externalBatteryAvg < 0)
nxtDisplayTextLine(3, "Ext Batt: OFF"); //External battery is off or not connected
else
nxtDisplayTextLine(3, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);

nxtDisplayTextLine(4, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage

nxtDisplayTextLine(5, "FMS Msgs: %d", joystick.msg.serialNumber); // Display Count of FMS messages
}

wait1Msec(200);
}
}
#endif


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// waitForStart
//
// Wait for the start of either the autonomous or tele-op phase. User program is running on the NXT
// but the phase has not yet started. The FMS (Field Management System) is continually (every 50 msec)
// sending information to the NXT. This program loops getting the latest value of joystick settings.
// When it finds that the FMS has started the phase, it immediately returns.
//
// Note: this has been modified to provide an optional escape hatch.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef USE_WAIT_FOR_START
#define USE_WAIT_FOR_START 1
#endif

#if USE_WAIT_FOR_START

// Define here in a simple way so that, at least, this file is self contained.
// See also display.h, where a better sol'n is provided.
#ifndef DisplayMessage
#define DisplayMessage(message) nxtDisplayCenteredTextLine(7, "%s", message)
#endif

// With this escape hatch, if the orange button is pressed within the first
// two seconds, the waitForStart() terminates. Note that the !(nNxtButtonTask = -1)
// at the end of OrangeButtonWaitBreak() is not a bug, but rather is intentionally
// both restoring the default button processing after the two seconds and returning
// false so as to not terminate the waitForStart().
#define OrangeButtonWaitInit() { nNxtButtonTask = -2; /* tell the NXT OS that we want the buttons */ }
#define OrangeButtonWaitCond() (MsSinceWaitForStart() <= OrangeButtonWaitTimeThreshold())
#define OrangeButtonWaitBreak() (OrangeButtonWaitCond() ? nNxtButtonPressed==kEnterButton: !(nNxtButtonTask = -1))
#define OrangeButtonWaitFeedback() { \
if (OrangeButtonWaitDoFeedback()) \
{ \
DisplayMessage(OrangeButtonWaitCond() ? "!wait==orange" : "waiting for start"); \
} \
}
#define OrangeButtonWaitDone() { \
nNxtButtonTask = -1; /* return buttons to their default processing */ \
DisplayMessage(DISPLAY_MESSAGE_DEFAULT); \
}

#ifndef OrangeButtonWaitDoFeedback
#define OrangeButtonWaitDoFeedback() true
#endif

#ifndef OrangeButtonWaitTimeThreshold
#define OrangeButtonWaitTimeThreshold() 0x7FFFFFFF
#endif

// Unless some tells us not to, we turn on the orange button processing
#ifndef WAITFORSTART_USE_ORANGE_BUTTON
#define WAITFORSTART_USE_ORANGE_BUTTON 1
#endif

#if WAITFORSTART_USE_ORANGE_BUTTON
#ifndef WaitForStartInit
#define WaitForStartInit() OrangeButtonWaitInit()
#endif
#ifndef WaitForStartBreak
#define WaitForStartBreak() OrangeButtonWaitBreak()
#endif
#ifndef WaitForStartLoop
#define WaitForStartLoop() OrangeButtonWaitFeedback()
#endif
#ifndef WaitForStartDone
#define WaitForStartDone() OrangeButtonWaitDone()
#endif
#endif

long msWaitForStart;
#define MsSinceWaitForStart() (nSysTime - msWaitForStart)

#define waitForStart() \
{ \
msWaitForStart = nSysTime; \
WaitForStartInit(); \
while (true) \
{ \
WaitForStartLoop(); \
getJoystickSettings(joystick); \
if (!joystick.msg.fWaitForStart) \
break; \
if (WaitForStartBreak()) \
break; \
} \
WaitForStartDone(); \
}

#endif

#endif




Auxilaryfunctions:

/* Function returns true if input variable "time" has elapsed. It resets itself automatically...
The wait function will probably return true the first time it is used.

Use the index to start separate timers for each thing you are tested.

Usage: Step up function for ball picker upper... increases power by 10 every 0.1 seconds. Full rampup in 1 second

#include "auxiliaryfunctions.h"
task main()
{
int power = 0;
while(true)
{
if(joy2Btn(6) == 1)
{
motor[motorA] = power;
if(wait(100, 0))
{
power = power + 10;
}
}
}
}

*/

long currenttime[10];
long previoustime[10] = {0,0,0,0,0,0,0,0,0,0};
long changetime[10];

bool wait(int time, int index)
{
currenttime[index] = nPgmTime;
changetime[index] = currenttime[index] - previoustime[index];
if(changetime[index] > time)
{
previoustime[index] = currenttime[index];
return true;
}
else
{
return false;
}
}



/* Function returns the speed of a Tetrix DC motor equipped with encoder in RPMs.

Inputs to the function are the motor you want to check, and an index. Use a different index for every
motor you are checking.

ex:
#include "auxiliaryfunctions.h"
task main()
{
while(true)
{
motor[motorD] = 10;
writeDebugStreamLine("%3.0f", SpeedCheck(motorD, 0));
wait1Msec(1000);
}
}


*/

float CurrentMotorPosition[8] = {0,0,0,0,0,0,0,0};
float PreviousMotorPosition[8] = {0,0,0,0,0,0,0,0};
float ChangeMotorPosition[8] = {0,0,0,0,0,0,0,0};
float CurrentTime[8] = {0,0,0,0,0,0,0,0};
float PreviousTime[8] = {0,0,0,0,0,0,0,0};
float ChangeTime[8] = {0,0,0,0,0,0,0,0};
float MotorSpeed[8] = {0,0,0,0,0,0,0,0};

float SpeedCheck(tMotor checkmotor, int motorindex)
{

CurrentMotorPosition[motorindex] = nMotorEncoder[checkmotor]; // Read Current Motor Position
ChangeMotorPosition[motorindex] = abs(CurrentMotorPosition[motorindex] - PreviousMotorPosition[motorindex]); //Read Change since last checked
CurrentTime[motorindex] = nPgmTime; // Read Total Time Passed since program began
ChangeTime[motorindex] = CurrentTime[motorindex] - PreviousTime[motorindex]; //Read Time Passed in this loop
MotorSpeed[motorindex] = ChangeMotorPosition[motorindex]/ChangeTime[motorindex]*1000/1440*60; //RPMs
PreviousMotorPosition[motorindex] = CurrentMotorPosition[motorindex];
PreviousTime[motorindex] = CurrentTime[motorindex];
return MotorSpeed[motorindex];

}



/* High Pass Filter: takes an input and clips it to zero if it is below threshold

Usage:
motor[motorD] = highpass(joy1_y1, 7); clips any values of joystick below 7 to zero.

Useful if you only want a motor to turn forwards with a deadband.

*/

int highpass(int power, int threshold)
{
if(power < threshold)
{
power = 0;
}
return power;
}


/* Low Pass Filter: takes an input and clips it to zero if it is above threshold

Usage:
motor[motorD] = highpass(joy1_y1, -7); clips any values of joystick above -7 to zero.

Useful if you only want a motor to turn backwards with a deadband.

*/

int lowpass(int power, int threshold)
{
if(power > threshold)
{
power = 0;
}
return power;
}


/* Deadband: clips an input to zero if the input is between -threshold and threshold.

ex: motor[motorA] = deadband(joy1_x1, 7);
*/

int deadband(int power, int threshold)
{
if(abs(power) < threshold)
{
power = 0;
}
return power;
}

Let's analyze one problem at a time. First, let's take a look at the crate lifter. The code is very hard to understand without detail comments about what buttons doing what. But if I understand the code correctly, it looks like you have assigned buttons 6 and 8 for the right crate lifter and buttons 5 and 7 for the left crate lifter. Don't know which buttons are for up and which are for down but it looks like in order to raise the crate, you need to pressed the two left and right "up buttons" simultaneously. When they are pressed, the left side will run with power 18 and the right side with power 20. So you are assuming the right motor is slightly weaker (slower). If I understand the code correctly, you have many problems. First, what if the left and right "up buttons" are not pressed quite at the same time. Then one side will be going up earlier than the other causing the lift to skew. Secondly, what if the there is an operator error where you pressed the up button for one side and the down button for the other side. This could be disasterous. Thirdly, the hard coded power difference between the left and right side will not remain constant. So even if you tune the difference very good at one point will not remain good at other times. I would make the following recommendations:
1. Use one button to raise the lifter for both left and right side. Use another button to lower the lifter for both left and right side.
2. Use the encoder to synchronize the speed of both sides dynamically and not hard coded power differences. Either doing this synchronization explicitly in your code using PID control or use the RobotC built-in motor synchronization feature. See the RobotC tutorial on this subject. (http://www.robotc.net/teachingmindstorm ... inth8.html).
3. If possible, I would design the lifter operating with one motor only, not with two. Our robot also has a lifter and we use one motor to lift both sides with one chain. So there is no worry of synchronizing two motors.

Hi,

Thanks for the idea. Given the geometry of our robot, it makes sense to run the two lifters separately with separate motors and with the buttons for control. It is not a problem if one goes up and the other goes down; in fact we need this to correct for minor differences in load on both sides of the robot and because the lifters are not professional; they can be "sticky" at times. Therefore even if the same power was applied to the motors, we saw that the motors became unaligned with was really problematic. The independence gives the driver the ability to make decisions and keep both arms as level as possible.

I agree with you that synchronizing the motors would be ideal; the nsynch motors function however does not work with tetrix motors (correct me if i'm wrong... the video you referenced and everything else i have seen which uses this function is for lego motors). we are not quite experienced enough yet to write our own PID to keep the motors completely aligned using the encoders. I also agree that using a single motor would probably be best; again at this stage in the game and given the geometry that is not a possibility.

The bigger problem, however, is the one written about originally; sometimes when we compile the program the two motors run at essentially the same speed. And sometimes the two motors run at drastically different speeds (so drastically different that the powers must be set to 5 and 60 in order to them to be going at the same speed). The loads the motors are seeing are virtually identical. We will try playing with swapping out motor controllers as this could be the issue. But in the way the wiring is setup, the problems are occuring for motors on three different motor controllers so it is difficult to point to one as the problematic controller.

PID control is not that difficult to write. Assuming you have reset the left lift and right lift encoders when they are perfectly horizontal at the bottom, when you apply power to the two motors, the difference between the two motor powers should be proportional to the difference in their encoder readings. For example:

Basically, if the left and right motors are perfectly synchronized, error will be zero and the motors will get equal power.
If the left side goes faster than the right, error will be a positive value. Then the left motor will get a smaller power and the right motor will get a larger power. However, you have to tune the Kp value. You probably can start with Kp = 0.1 and go from there.
Note that since I don't know about the polarities of your motors and encoders, you may need to adjust the signs of the code above.
With PID contol monitoring the two motor speed, you should not have problems with one running faster than the other at different times.

Regarding your tank drive problem, like I said in my previous reply, the friction on both sides of the lift may be different at different time. For example, if the lift has been skewed, you probably have a mechanical binding on one or both sides. The side that got stuck will have tremendous resistance causing that side to slow down or stopped while the non-stuck side will keep going. Eventually, if enough skewing happened, both sides will get stuck. While the lift is stuck, your lifter motors will draw tremendous current (stall current) that probably drain down the battery. This probably affects the tank drive motors. That's my guess on what happened. With PID control on the lifter, you may or may not fix this problem. The hope is that if the lifter is running near perfectly horizontal, it is less likely to bind mechanically and so it will run smoothly and not drawing stall current. In addition, you may need to lubricate your lifter tracks to minimize friction and binding.

So we rewired the robot completely, putting the two drive motors onto a single motor controller, the two crate lifters onto a single motor controller, the servos on one controller and the ball lifter motors on the last motor controller. Here is what we tried:

We put each of the motor controllers on a different port (drive on S1, ball lifter on S2, crates lifters on S3). Incidentally, it wouldn't let us put the servo on S4; didn't allow us to add servos in the motors/sensors setup (it would allow us to edit in the wizards but never updated the pragma config). So on S1 we had drive/servos, S2 ball lifter, S3 crate lifters.

We compiled, ran, worked fine.

Then we tried to daisy chain all the controllers off S1 (drive, ball lifter, crate lifters, servo in that order), ran again, and saw the problem reoccur with both the crate lifters and the turbo not working. So that means we had replicated the problem with motors setup wired to different motor controllers.

Next we swapped out the NXT and put the program on a new NXT with the daisy chained wiring, and the problem persisted... so it's not the NXT.

We moved the motor controllers back to different ports as before (drive on S1, ball lifter on S2, crates lifters on S3) because that configuration seemed to be working. This time, the lifters moved at the same speed, but the turbo/slow settings for the drive motors didn't work. This was the first time we had seen the two problems (lifters/tank drive) isolated from each other. So then we decided to see if we could get the problem to move with the port; we switched S1 and S3 and the program worked.

I'm really having a hard time... i can't see any pattern emerging...

Given that at one point we saw the problem occurring only with the tank drive motors, we could try swapping out that motor controller... that's about the only thing i can think of at this point.

This may be totally off-base, but have you tried replacing the NXT cables? I read that the daisychain addressing of the HiTechnic controllers is achieved in some fashion using the analog line on the port. If you have a bad cable I assume that could play havoc with the addressing and create some very peculiar behavior.