#pragma config(Sensor, S1, NoBttn, sensorTouch)
#pragma config(Sensor, S2, YesBttn, sensorTouch)
#pragma config(Sensor, S3, LightSens, sensorLightInactive)
#pragma config(Sensor, S4, HTIRL, sensorI2CCustom)
#pragma config(Motor, motorC, TumblrMtr, tmotorNormal, PIDControl, reversed, encoder)

#include "drivers/HTIRL-driver.h";

void initialize();
void InitializePF();

const ubyte IsConnected[1] = {1};
const ubyte IAmConnected[1] = {2};
const ubyte DoneSorting[1] = {3};
const ubyte StartSorting[1] = {4};
const ubyte Okay[1] = {5};
const ubyte StopConveyors[1] = {6};


ubyte SentMessage[1];

tPFmotor Tread1AndLights = pfmotor_S4_C1_A;
tPFmotor Tread2 = pfmotor_S4_C1_B;
tPFmotor DispenserMtr = pfmotor_S4_C2_A;
tPFmotor PneumSwtch = pfmotor_S4_C2_B;
tPFmotor BrickScannerDisp = pfmotor_S4_C3_A;
tPFmotor ColorScanAlign = pfmotor_S4_C3_B;

ePWMMotorCommand Tread1AndLightsFwd = MOTOR_FWD_PWM_7;
ePWMMotorCommand Tread2Fwd = MOTOR_FWD_PWM_7;
ePWMMotorCommand DispenserMtrDisp = MOTOR_FWD_PWM_7;
ePWMMotorCommand PneumClawUp = MOTOR_FWD_PWM_7;
ePWMMotorCommand PneumClawDwn = MOTOR_REV_PWM_7;
ePWMMotorCommand BrickScannerDispExt = MOTOR_FWD_PWM_3;
ePWMMotorCommand BrickScannerDispRtr = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignExt = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignRtr = MOTOR_FWD_PWM_3;

task main()
{
  initialize();
 
  while(SentMessage[0] != DoneSorting[0])
  {
    cCmdMessageRead(SentMessage[0], 1, 0);
   
    switch(SentMessage[0])
    {
      case StartSorting[0]:
        PFMotor(Tread1AndLights, Tread1AndLightsFwd);
        PFMotor(Tread2, Tread2Fwd);
      break;
      case StopConveyors[0]:
        PFMotor(Tread1AndLights, MOTOR_BRAKE);
        PFMotor(Tread2, MOTOR_BRAKE);
      break;
    }
  }
}

void initialize()
{
  while(SentMessage[0] != IsConnected[0]){cCmdMessageRead(SentMessage[0], 1, 0);}
  cCmdMessageWriteToBluetooth(IAmConnected[0], 1, 0);
 
  InitializePF();
}
void InitializePF()
{
  PFMotor(ColorScanAlign, ColorScanAlignRtr);
  wait1Msec(500);
  PFMotor(ColorScanAlign, MOTOR_BRAKE);
 
  PFMotor(BrickScannerDisp, BrickScannerDispRtr);
  wait1Msec(500);
  PFMotor(BrickScannerDisp, MOTOR_BRAKE);
}

#define START_SORTING          4
…
switch (SendMessage[0])
{
    case START_SORTING:
        break;
    ...
}