/************************************************************************/
/*                                                                      */
/* Program Name: NRLink-Nx-RobotC-PF-Motor-Sample.c                     */
/* ===========================                                          */
/*                                                                      */
/* Copyright (c) 2008 by mindsensors.com                                */
/* Email: info (<at>) mindsensors (<dot>) com                           */
/*                                                                      */
/* This program is free software. You can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; version 3 of the License.              */
/* Read the license at: http://www.gnu.org/licenses/gpl.txt             */
/*                                                                      */
/************************************************************************/

#include "NRLink-lib.c"

const tSensors NRLinkPort         = S1;        // Connect NRLink sensor to this port!!
const ubyte NRLinkID              = 0x02;

/////////////////////////////////////////////////////////////////////////////
//
//      Run some commands and macro to control PF Motors using NRLink.
//
/////////////////////////////////////////////////////////////////////////////

task main()
{

	byte Data[10];

	nI2CBytesReady[NRLinkPort] = 0;


	SensorType[NRLinkPort] = sensorI2CCustom9V;
	nxtDisplayTextLine(0,"mindsensors.com");
	nxtDisplayTextLine(1,"NRLink");
	NRLinkSendCommand(NRLinkPort, NRLinkID, NRLinkFlush);
	NRLinkSendCommand(NRLinkPort, NRLinkID, NRLinkDefault);
	NRLinkSendCommand(NRLinkPort, NRLinkID, NRLinkLongRange);
	NRLinkSendCommand(NRLinkPort, NRLinkID, 'P');

	while(1)
	{
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_A_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_A_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_A_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_A_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_B_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_B_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_B_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch1_B_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_A_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_A_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_A_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_A_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_B_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_B_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_B_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch2_B_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_A_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_A_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_A_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_A_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_B_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_B_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_B_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch3_B_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_A_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_A_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_A_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_A_Brake);
		wait10Msec(20);

		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_B_FWD);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_B_Float);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_B_REV);
		wait10Msec(20);
		NRLinkRunMacro(NRLinkPort, NRLinkID, PF_Motor_Ch4_B_Brake);
		wait10Msec(20);

	}
	StopAllTasks();

}