/*=====================================================================*/

void main(void)
{
	int stpRetVal = ERROR;

	// init HW sequence
	startSeq((INITSEQ*)&initHW[0]);
	puts("HW init end\n\r\r");

	// init SW sequence
	startSeq((INITSEQ*)&initSW[0]);
	puts("SW init end\n\r\r");

	// first loop, wait 1sec to enter special debug mode on serial line
	firstWaitCycle();

	printMenu();
	refreshWatchDog();

	/* main loop */
	for(;;)
	{
		if(f2KHz == 1)
		{
			f2KHz = 0;

			if(doMoving())
			{
				doStepMotor(REAR_MOT_COMP);
				doStepMotor(REAR_MOT_REB);
				doStepMotor(FRONT_MOT_COMP);
				doStepMotor(FRONT_MOT_REB);
			}

			if(f1KHz == 1)
			{
				f1KHz = 0;

				stpRetVal = doStp();

				if(f100Hz == 1)
				{
					f100Hz = 0;

					doReadSensor_100Hz();
					doCalc_IC(IC_SPEED);
					doCalc_IC(IC_RPM);
					doReadInput_100Hz(icChan[IC_SPEED].value);
					mapsInterpolate(icChan[IC_SPEED].value);
					txChanCan_100Hz();
					log232();
					doAcqAcc();

					if(f10Hz == 1)
					{
						f10Hz = 0;

						refreshWatchDog();

						if (stpRetVal == ERROR)
							doMenu();

						doDiagn();				// show led diagn state
						canWatchDog();

					}	/* end 10Hz */
				}	/* end 100Hz */
			}	/* end 1KHz */
		}	/* end 2KHz */
	}
}