Advanced Motor Control In RobotC

Wanna know one way to control three motors with each having different speeds, directions and targets?  Here's how.  Copy the code after the break and create a file in RobotC.  Hook up three motors and run it.  If there is enough interest in the code, I will go deeper into dissecting it.

I apologize about the formatting and tabification of the code.  Blogger is terrible for that kind of thing.

//Format: {Arraylines, motorA_Pos, motorA_Pow, motorB_Pos, motorB_Pow, motorC_Pos, motorC_Pow}

int Array[][]=
{
{ 5, 50, 50, 50, 50, 200, 10}, //Line 0 **
{ 0, 100, 10, -50, 50, 100, 20}, //Line 1
{ 0, -50, 75, 50, 50, 50, 30}, //Line 2
{ 0, 200, 100, -50, 50, 0, 40}, //Line 3
{ 0, 400, 5, 50, 50, -50, 50}, //Line 4
{ 2, -400, 60, -50, 50, -100, 60}, //Line 5 **
{ 0, 0, 45, 50, 50, -200, 70}, //Line 6
{ 2, 60, 50, -50, 50, -100, 80}, //Line 7 **
{ 0, 10, 30, 50, 50, -50, 90}, //Line 8
{ 3, 600, 90, -50, 50, 0, 100}, //Line 9 **
{ 0, 50, 40, 50, 50, 200, 10}, //Line 10
{ 0, 25, 60, -50, 50, 0, 10}, //Line 11
};

void MoveAllMotors(int ArrayLine);

task main
{
bFloatDuringInactiveMotorPWM = false;
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
nSyncedMotors = synchNone;
nMaxRegulatedSpeedNxt = 750;

nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;

MoveAllMotors(0);
MoveAllMotors(5);
MoveAllMotors(7);
MoveAllMotors(9);
MoveAllMotors(5);
MoveAllMotors(7);
}

void MoveAllMotors(int ArrayLine)
{
int motorA_Dir, motorB_Dir, motorC_Dir;
int motorA_Target, motorB_Target, motorC_Target;
int motorA_CurrentPosition, motorB_CurrentPosition, motorC_CurrentPosition;
int motorA_Power, motorB_Power, motorC_Power;
bool motorA_Status = false, motorB_Status = false, motorC_Status = false;

int UpperLimit = ArrayLine + Array[ArrayLine][0]; // Calculate the number of moves to make
for(int Index = ArrayLine; Index < UpperLimit; Index++)
{
//Read the data from the array
motorA_Target = Array[Index][1];
motorA_Power = Array[Index][2];

motorB_Target = Array[Index][3];
motorB_Power = Array[Index][4];

motorC_Target = Array[Index][5];
motorC_Power = Array[Index][6];

nxtDisplayCenteredBigTextLine(0, "IN:%d", Index);

//Record the current position
motorA_CurrentPosition = nMotorEncoder[motorA];
motorB_CurrentPosition = nMotorEncoder[motorB];
motorC_CurrentPosition = nMotorEncoder[motorC];

//Process the motor directions relative to the last position
if (motorA_Target == motorA_CurrentPosition) motorA_Dir = 0;
else if (motorA_Target > motorA_CurrentPosition) motorA_Dir = 1;
else if (motorA_Target < motorA_CurrentPosition) motorA_Dir = -1;

if (motorB_Target == motorB_CurrentPosition) motorB_Dir = 0;
else if (motorB_Target > motorB_CurrentPosition) motorB_Dir = 1;
else if (motorB_Target < motorB_CurrentPosition) motorB_Dir = -1;

if (motorC_Target == motorC_CurrentPosition) motorC_Dir = 0;
else if (motorC_Target > motorC_CurrentPosition) motorC_Dir = 1;
else if (motorC_Target < motorC_CurrentPosition) motorC_Dir = -1;

//Initiate motor statuses
motorA_Status = false;
motorB_Status = false;
motorC_Status = false;

//Start the motors
motor[motorA] = motorA_Dir * motorA_Power;
motor[motorB] = motorB_Dir * motorB_Power;
motor[motorC] = motorC_Dir * motorC_Power;

//Run loop until all the motors have completed their moves (until all statuses are true)
while (motorA_Status==false || motorB_Status==false || motorC_Status==false)
{
switch (motorA_Dir)
{
case -1: // Move down
if (nMotorEncoder[motorA] < motorA_Target){
motor[motorA]=0;
motorA_Dir = 0; //Makes the next loop "case 0"
}
break;
case 0: // Don't Move (brake)
motorA_Status = true;
break;
case 1: // Move Up
if (nMotorEncoder[motorA] > motorA_Target){
motor[motorA]=0;
motorA_Dir = 0; //Makes the next loop "case 0"
}
break;
}

switch (motorB_Dir)
{
case -1: // Move down
if (nMotorEncoder[motorB] < motorB_Target){
motor[motorB]=0;
motorB_Dir = 0; //Makes the next loop "case 0"
}
break;
case 0: // Don't Move (brake)
motorB_Status = true;
break;
case 1: // Move Up
if (nMotorEncoder[motorB] > motorB_Target){
motor[motorB]=0;
motorB_Dir = 0; //Makes the next loop "case 0"
}
break;
}

switch (motorC_Dir)
{
case -1: // Move down
if (nMotorEncoder[motorC] < motorC_Target){
motor[motorC]=0;
motorC_Dir = 0; //Makes the next loop "case 0"
}
break;
case 0: // Don't Move (brake)
motorC_Status = true;
break;
case 1: // Move Up
if (nMotorEncoder[motorC] > motorC_Target){
motor[motorC]=0;
motorC_Dir = 0; //Makes the next loop "case 0"
}
break;
}
}
}
}