Exemplo n.º 1
0
/**
 * \brief Displays light level on LED.
 * Used for self-test.
 *
 * Press button to exit the test
 */
void BrightnessLevel()
{
	uint8_t level;
	uint8_t button;

	do
	{
		button = ButtonCheck();
		level = GetLight();

		level /= 32; //convert to 8 levels

		/* This displays digit from 0 to 7
		ledPutc('0'+level); */


		/* This is a test code to display ADC value on LED instead of bar graph
		 * Don't forget to comment out "level /= 32;" line
		sprintf(g_TextBuffer," %d", level);
		g_TextBufferLen = strlen( g_TextBuffer );
		ShowText();
		*/


		ledPutc(SYMBOL_LIGHT_LEVEL + 7-level);

		_delay_ms( 20 );
	} while( button != BUTTON_DOWN );
}
Exemplo n.º 2
0
/**
 * Display text stored in EEPROM memory using current font.
 *
 * \param szText Text to display in EEPROM. Must be terminated by 0 or 0xFF.
 *
 * \retval BUTTON_SHORT Button pressed short
 * \retval BUTTON_LONG Button pressed long
 * \retval BUTTON_NONE Timeout
 */
uint8_t ledPuts_EE( const uint8_t* szText )
{
	int offset = 0;
	uint8_t key;

	//copy data from EEPROM
	for(g_TextBufferLen=0;g_TextBufferLen<TEXTBUFFER_SIZE;g_TextBufferLen++)
	{
		g_TextBuffer[g_TextBufferLen] = eeprom_read_byte( szText + g_TextBufferLen ); //read byte from eeprom
		if( 0 == g_TextBuffer[g_TextBufferLen] || 0xFF /*EEPROM not programmed*/ == g_TextBuffer[g_TextBufferLen] )
		{
			break; //end of string
		}
	}

	//scroll once entire text
	do
	{
		ScrollLeft(g_TextBuffer, g_TextBufferLen, &offset );

		key = ButtonCheck();
		if( key == BUTTON_SHORT || key == BUTTON_LONG )
		{
			return key;
		}

		_delay_ms(20);

	} while( offset );

	return BUTTON_NONE;
}
Exemplo n.º 3
0
void updateSetpoints(short* yawSetpoint, short* heightSetpoint)
{
	// Yaw
	if (ButtonCheck(LEFT))
	{
		*yawSetpoint -= 15;
	}
	else if (ButtonCheck(RIGHT))
	{
		*yawSetpoint += 15;
	}

	if (*yawSetpoint > 300)
	{
		*yawSetpoint = 300;
	}
	else if (*yawSetpoint < -300)
	{
		*yawSetpoint = -300;
	}


	// Height
	if (ButtonCheck(DOWN) )//&& *heightSetpoint > 0)
	{
		*heightSetpoint -= 10;
	}
	else if (ButtonCheck(UP))
	{
		*heightSetpoint += 10;
	}

	if (*heightSetpoint > 100)
	{
		*heightSetpoint = 100;
	}
	else if (*heightSetpoint < 0)
	{
		*heightSetpoint = 0;
	}
	return;
}
Exemplo n.º 4
0
///////////////////////////////////////////////////////////////////////////////
////////////////////////////////    MAIN    ///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
int main(void)
{
	IntMasterDisable();

	//=====SYSTEM PERIPHERAL INITIALIZATION=====

	// Initialize real time clock
	mRTCInit(RTC_RATE_HZ);

	// Millisecond timekeeping variable
	int time;

	//Add periodic tasks to be executed by systick
	mRTCAddTask(heightSample, HEIGHT_SAMPLE_RATE_HZ);

	// Set up display
    mDisplayInit(1000000);
    mDisplayLine("     Waiting...    ", 0, 5, 15);

	// Set up buttons
    ButtonInit();

	// Set up PWM
	mPWMInit();
	mPWMEnable(PWM4, false); // tail rotor, yaw control.
	mPWMEnable(PWM1, false); // main rotor, height control.

	//=========CONTROL INITIALIZATION========


	//-----altitude PID control------

    // Initialize altitude module
	heightInit();

	// =========PID parameters========================

	//Phils
	float H_kp = 1;float H_ki = 1.5;float H_kd = -0.5;

	short heightPIDOffset = 40;

	float Y_kp = 0.6;float Y_ki = 0;float Y_kd = 0;

	short YawPIDOffset = 0;//50;


	// Windup regulator
	float H_windup_limit = 10;
	if (H_ki)
		H_windup_limit /= H_ki;

	// height PID controller
	pid_t heightPID;
	PIDInit(&heightPID);
	PIDSet(&heightPID, H_kp, H_ki, H_kd, H_windup_limit); // set PID constants

	// height PID control variables
	//35; // PID out offset such that the rotor is at near-takoff speed
	short height = 0;
    short heightSetpoint = 0; // in degrees
    short heightError = 0;
    short heightPIDOut = 0;


	//-----yaw PID control-------

	// Initialise Yaw decoder module
	yawInit(); // yaw monitor



	float Y_windup_limit = 20; // Maximum integral contribution to PID output value
	if (Y_ki)
		Y_windup_limit /= Y_ki;	// devide by Y_ki to find maximum value in terms of error

	// Yaw PID controller
	pid_t yawPID;
	PIDInit(&yawPID);
	PIDSet(&yawPID, Y_kp, Y_ki, Y_kd, Y_windup_limit); // set PID constants

	// yaw PID control variables
	short yaw = 0;
	short yawSetpoint = 0;
    short yawError = 0;
    short yawPIDOut = 0;


    //
    // Enable interrupts to the processor.
    IntMasterEnable();


    short takeOffFlag = false;
	while (!takeOffFlag)
	{
		if (ButtonCheck(SELECT))
		//if (GPIOPinRead(GPIO_PORTG_BASE, 0x80))
		{
			takeOffFlag = true;
		}
	}

	// Reset setpoints to current position
	heightSetpoint = heightGet();
	yawSetpoint = yawGetAngle();



	mPWMEnable(PWM4, true); // tail rotor, yaw control.
	mPWMEnable(PWM1, true); // main rotor, height control.


	//spin up routine
	//spinUp(heightPIDOffset, yawPID);

	// Reset clock to zero for effective helicopter launch time
	mRTCSet(0);

	while (1)
	{
		//mDisplayClear();

		time = mRTCGetMilliSeconds();

		// Update Setpoints
		updateSetpoints(&yawSetpoint, &heightSetpoint);

		// ==================PID Control=================
		if ((time % (RTC_RATE_HZ/PID_RATE_HZ)) /*1000/(float)PID_RATE_HZ*/ == 0)
		{
			//
			// ~~~~~~~~~~~~~~~~~ HEIGHT PID ~~~~~~~~~~~~~~~~

			height = heightGet();

			heightError = heightSetpoint - height;

			heightPIDOut = PIDUpdate(&heightPID, heightError, 1.00 / (float)PID_RATE_HZ) + heightPIDOffset;

			if (heightPIDOut > 79)
				//heightPIDOut = 79;
			if (heightPIDOut < 2)
				heightPIDOut = 2;

			mPWMSet(PWM1, (unsigned short)heightPIDOut);


			//
			// ~~~~~~~~~~~~~~~~~~ YAW PID ~~~~~~~~~~~~~~~~~~~
			yaw = yawGetAngle();

			yawError = yaw - yawSetpoint;

			yawPIDOut = PIDUpdate(&yawPID, yawError, 1.00 / (float)PID_RATE_HZ) + YawPIDOffset;

			if (yawPIDOut > 79)
				yawPIDOut = 79;
			if (yawPIDOut < 2)
				yawPIDOut = 2;

			mPWMSet(PWM4, (unsigned short)yawPIDOut);

			// ===============================================
		}

		// RTC_RATE_HZ - PID_RATE_HZ
		if (( (time) % 10) == 0)
		{
			mDisplayLine("time:%.6d mS", time, 0, 7);
			mDisplayLine("Yaw  = %.4d'    ", (int)yaw, 1, 15);
			mDisplayLine("YSet = %.4d'    ", (int)yawSetpoint, 2, 15);
			mDisplayLine("YErr = %.4d'    ", (int)yawError, 3, 15);
			mDisplayLine("YOut = %.4d'    ", (int)yawPIDOut, 4, 15);
			mDisplayLine("height = ~%.3d ", (int)height, 5, 15);
			mDisplayLine("Hset   = %.4d   ", (int)heightSetpoint, 6, 15);
			mDisplayLine("Herr   = %.4d   ", (int)heightError, 7, 15);
			mDisplayLine("Hout   = %.4d   ", (int)heightPIDOut, 8, 15);
		}

		// should put this as part of the main while loop condition
		//if (ButtonCheck(SELECT))
		//{
		//	spinDown();
		//}

	}
}
void ButtonGuardThread(void* pvParameters)
{
	unsigned long ulTick = 0;
	s_ulIdleTick = 0;
	unsigned long ulLongKeepOff = 0;
	
	unsigned long ulChrgTick = 0;
	bool bEnableRunning = true;
	while(1) {
		ulTick++;
		
		//	按钮状态检测
		int nBtnAction = ButtonCheck();
		if (nBtnAction != BTN_NOACTION) {
			s_ulIdleTick = 0;
			
			if ( (nBtnAction==BTN_PUSHDOWN) && g_bRunning ) {
				g_bRunning = false;
				MotorRun(g_bRunning);
				bEnableRunning = false;
			}
			else if ( (nBtnAction==BTN_POPUP) && !g_bRunning ) {
				if (!bEnableRunning) {
					bEnableRunning = true;
				}
				else {
					if (g_bRunable) {	//	如果不在SimpleLink操作阶段则不能运行探头
						g_bRunning = true;
						MotorRun(g_bRunning);
					}
				}
			}
			
			ulLongKeepOff = 0;	//	长按关机计数
		} else {	//	长按关机状态下关闭电源
			if ( s_nStatOut == BTN_UP) {
				ulLongKeepOff = 0;
			} else {
				++ulLongKeepOff;
				if (ulLongKeepOff/100 > 8) {	//	长按8秒关闭电源
					MotorRun(false);
					TurnOff();
				}
			}
			
			//	注:
			//		长按关机是为了避免运输工程中,如果探头的按钮受到挤压而开机;
			//	在这种状态下,按钮控制芯片并不会在一定时间后主动关闭电源,从而
			//	会导致电源耗尽或者其他潜在风险。
			//		故而解决的方法是检测按钮是否长时间(8S)处于未知或者按下
			//	状态,一旦检测到则关闭电源。
			//												吴文斌
			//											2015 - 3 - 30
		}
		
		//	充电状态检测
		if (IsCharging()) {
			ulChrgTick++;
			if (ulChrgTick > 20) {			
				TRACE("Begin Charging...\n\r");	
				g_bRunning = false;
				MotorRun(g_bRunning);
				TurnOff();
			}
		}
		else {
			ulChrgTick = 0;
		}
		
		
		//	待机时间检测
		++s_ulIdleTick;
		if (	(s_ulIdleTick/100/60 > 10)	//	连续运行10分钟则停止运行
			&&	g_bRunning )
		{
			TRACE("10min to Stop Running...\n\r");
			g_bRunning = !g_bRunning;
			MotorRun(g_bRunning);
		}
		if ( s_ulIdleTick/100/60 > 15) {		//	待机时间超过15分钟则关闭电源
			TRACE("15min to Power Off...\n\r");
			TurnOff();
		}
		
		
		vTaskDelay(10/portTICK_PERIOD_MS);
	}
}