/*  RobotC-HTLink-Power Function PWM Mode
    Version: 0.1.6
    Date: 14/08/08
    Author: Mike Hatton
    Contact: Parax77ATgooglemailDOTCom
    Usage:
      PFComboPWM(Channel(0-3), OutputRed(0-16), OutputBlue(0-16))
      Function populates PFB[] Array
      use sendI2CMsg to send PFB[] to HTLink Sensor
      See task main() below for example
    Process: Short explanation of how the bits in the byte array are set using the PFbyteBitCounter (PFBBi).
      80 Bits in 10 Bytes, Target Byte = PFBBi/8 Target Bit in Byte = PFBBi Mod 8 (Note In PF Message Target Byte is After StartBit Hence +4)
      To add Target Bit to Target Byte Use BitWise OR for 128>>TargetBit
      eg Set Target Bit 12 (PFBBi): Byte 12/8 = 1, bit 12%8 = 4, = 0x80>>4 = 00001000 use OR to set Bit 4 in Byte 1
    Changes: Removed Pow Function replaced with Bit Shift */

//PFByteArray for I2C msg
byte PFB[17];

//PFB[] Creation Function
void PFComboPWM(byte Channel, byte OutR, byte OutB)
{
	byte PFBi = 0; //PFByte Counter
	byte PFBBi = 0; //PFByte Bit Counter
	byte N[4]; //Nibble Array
	byte Ni = 0; //Nibble Counter
	byte NBi = 0; //Nibble Bit Counter
	//Clear PFB[] Array
	for (PFBi = 0; PFBi < 17; PFBi++) {
		PFB[PFBi] = 0x00;
	}
	PFBi = 0;
	//Populate Nibbles
	N[0] = 0x4+Channel; //PWM Mode + (0-3)
	N[1] = OutB; //(0-15)
	N[2] = OutR; //(0-15)
	N[3] = 0xF; //XOR CRC
	N[3] = N[3]^N[0]^N[1]^N[2];//XOR CRC
	//Write PFB[0-3] (Fixed)
	PFB[0] = 16; //PFByte Length
	PFB[1] = 0x02; //Sensor Address
	PFB[2] = 0x42; //Write to Address
	PFB[3] = 0x80; //Start Bit
	//Write PFB[4-13] From Nibbles
	//For Nibble 1 to 4
	for (Ni = 0 ; Ni < 4 ; Ni++) {
		//For Nibble Bit = 1 to 4
		for (NBi = 0 ; NBi < 4 ; NBi++) {
			//Write NibbleBit to PFByte at PFByteBit Location
			PFB[4+(PFBBi/8)] = PFB[4+(PFBBi/8)]|(0x80>>(PFBBi%8));
			//Check NibBit & Advance PFBBi Location along Bit Sequence
			if (N[Ni]&(0x8>>NBi)) {
				PFBBi = PFBBi + 5; // add High = 5
			}
			else {
				PFBBi = PFBBi + 3; //add Low = 3
			}	//Next NibBit
		}	//Next Nibble
	}
	//Add Stop Bit, at Last PFBBi Location, Trailing Bits are Zero
	PFB[4+(PFBBi/8)] = PFB[4+(PFBBi/8)]|(0x80>>(PFBBi%8));
	//Write PFB[14-16] (Fixed)
	PFB[14] = 0x11; //Length
	PFB[15] = 0x02; //PF Mode
	PFB[16] = 0x01; //Xmit
}

task main()
{
	//Send PFBytes[] to S1
	SensorType[S1] = sensorI2CCustom9V;
	nI2CBytesReady[S1] = 0;
	int OutR = 0;
	int OutB = 0;
  int ch = 0;

  PlaySound(soundUpwardTones);
  while (bSoundActive) {}

	// Test the motors and recievers.
	for (OutR = 0; OutR < 16; OutR++) {
		OutB = 15 - OutR;
		for (ch=0;ch<4;ch++){
    	PFComboPWM( ch, OutB, OutR);
  		while (nI2CStatus[S1] == STAT_COMM_PENDING) {}
	  	sendI2CMsg(S1, PFB[0], 0); // Send PFBytes
	  	wait1Msec(20);
	  }
	  wait10Msec(60);
	}
  PlaySound(soundDownwardTones);
  while (bSoundActive) {}
}