Пример #1
0
void LED_UP(void)          //LED点亮
{
	P0DIR &= ~0X80;       //set p0.7 output
	
  LED3_ON();
	
 	//P0DIR |= 0X80;       //关闭GPIO
}
Пример #2
0
void ButtonRightHandler(void)
{
	if (system_GetState() == SYSTEM_CALIB_SENSOR)
	{
		switch(IR_Calib_Step)
		{
			case 0:
				LED1_ON();
				LED2_OFF();
				LED3_OFF();
				IR_set_calib_value(IR_CALIB_BASE_LEFT);
				IR_set_calib_value(IR_CALIB_BASE_RIGHT);
				break;
			case 1:
				IR_set_calib_value(IR_CALIB_BASE_FRONT_LEFT);
				IR_set_calib_value(IR_CALIB_BASE_FRONT_RIGHT);
				LED1_OFF();
				LED2_ON();
				LED3_OFF();
				break;
			case 2:
				IR_set_calib_value(IR_CALIB_MAX_LEFT);
				LED1_ON();
				LED2_ON();
				LED3_OFF();
				break;
			case 3:
				IR_set_calib_value(IR_CALIB_MAX_RIGHT);
				LED1_OFF();
				LED2_OFF();
				LED3_ON();
				break;
			case 4:
				IR_set_calib_value(IR_CALIB_MAX_FRONT_LEFT);
				IR_set_calib_value(IR_CALIB_MAX_FRONT_RIGHT);
				LED1_ON();
				LED2_OFF();
				LED3_ON();
				break;
		}
		IR_Calib_Step++;
		IR_Calib_Step %= 4;
	}
}
Пример #3
0
void system_Process_System_State(void)
{
	switch (system_GetState())
	{
		case SYSTEM_POWER_UP:
			break;
		case SYSTEM_INITIALIZE:
			break;
		case SYSTEM_CALIB_SENSOR:
			break;
		case SYSTEM_SAVE_CALIB_SENSOR:
			break;
		case SYSTEM_ESTIMATE_MOTOR_MODEL:
			ProcessSpeedControl();
			break;
		case SYSTEM_SAVE_MOTOR_MODEL:
			break;
		case SYSTEM_WAIT_TO_RUN:
			break;
		case SYSTEM_RUN_SOLVE_MAZE:
			pid_Wallfollow_process();
			ProcessSpeedControl();
			break;
		case SYSTEM_RUN_IMAGE_PROCESSING:
			LED1_ON();
			ProcessSpeedControl();
			break;
		case SYSTEM_ERROR:
			speed_Enable_Hbridge(false);
			system_Enable_BoostCircuit(false);
			IntMasterDisable();
			while (1)
			{
				LED1_ON();
				LED2_ON();
				LED3_ON();
				ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
				LED1_OFF();
				LED2_OFF();
				LED3_OFF();
				ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
			}
//			break;
	}
}
Пример #4
0
void ButtonHandler(void)
{
	switch (system_GetState())
	{
		case SYSTEM_INITIALIZE:
			speed_Enable_Hbridge(false);
			system_SetState(SYSTEM_CALIB_SENSOR);
			IR_Calib_Step = 0;
			LED1_ON();
			LED2_ON();
			LED3_ON();
			break;
		case SYSTEM_CALIB_SENSOR:
			speed_Enable_Hbridge(false);
			system_SetState(SYSTEM_SAVE_CALIB_SENSOR);
		case SYSTEM_SAVE_CALIB_SENSOR:
			system_SetState(SYSTEM_ESTIMATE_MOTOR_MODEL);
			speed_Enable_Hbridge(true);
			speed_set(MOTOR_LEFT,500);
			speed_set(MOTOR_RIGHT, 500);
			break;
		case SYSTEM_ESTIMATE_MOTOR_MODEL:
//			system_SetState(SYSTEM_SAVE_MOTOR_MODEL);
			system_SetState(SYSTEM_WAIT_TO_RUN);
			speed_Enable_Hbridge(false);
			break;
		case SYSTEM_WAIT_TO_RUN:
			speed_Enable_Hbridge(true);
			system_SetState(SYSTEM_RUN_SOLVE_MAZE);
			break;
		case SYSTEM_RUN_SOLVE_MAZE:
		case SYSTEM_RUN_IMAGE_PROCESSING:
			system_SetState(SYSTEM_WAIT_TO_RUN);
			speed_Enable_Hbridge(false);
			break;
		default:
			break;
	}
}
Пример #5
0
void system_Process_System_State(void)
{
	switch (system_GetState())
	{
		case SYSTEM_POWER_UP:
			break;
		case SYSTEM_INITIALIZE:
			break;
		case SYSTEM_ESTIMATE_MOTOR_MODEL:
			break;
		case SYSTEM_SAVE_MOTOR_MODEL:
			break;
		case SYSTEM_WAIT_TO_RUN:
			break;
		case SYSTEM_RUN_BALANCE:
			loop();
			break;
		case SYSTEM_RUN_IMAGE_PROCESSING:
			LED1_ON();
			break;
		case SYSTEM_ERROR:
			speed_Enable_Hbridge(false);
			system_Enable_BoostCircuit(false);
			IntMasterDisable();
			while (1)
			{
				LED1_ON();
				LED2_ON();
				LED3_ON();
				ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
				LED1_OFF();
				LED2_OFF();
				LED3_OFF();
				ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
			}
	}
}
Пример #6
0
//*****************************************************************************
//
//! Control two motor to make robot turn back 180 degree.
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of left motor.
//! \param NumPulse is the total pulse of two encoder after turn
//! \param resetEnc is the reset value for encoder after turning back
//! \return true if finish
//!			false if not
//
static bool TurnBack(int fwdPulse, int avrSpeedLeft,int avrSpeedRight,int turnPulse,
		int resetEnc)
{
	LED1_ON();LED2_ON();LED3_ON();
	switch (CtrlStep)
	{
	case 1:
	{
		posLeftTmp = qei_getPosLeft();
		avrSpeedTmp = avrSpeed;
		CtrlStep++;
	}
	case 2://go forward a litte bit
	{

		if (abs(qei_getPosLeft()-posLeftTmp)<fwdPulse)
		{
			avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
					+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
			if (isWallRight)
				pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
			else if (isWallLeft)
				pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_LEFT);
			else
			{
				speed_set(MOTOR_RIGHT, avrSpeed);
				speed_set(MOTOR_LEFT, avrSpeed);
			}
		}
		else
		{
			pid_reset(&pid_wall_left);
			pid_reset(&pid_wall_right);
			forwardUpdate();
			CtrlStep++;
			avrSpeed = avrSpeedTmp;
		}
		break;
	}
	case 3:
		posLeftTmp=qei_getPosLeft();
		posRightTmp=qei_getPosRight();
		CtrlStep++;
	case 4://turing 90 degree
	{
#ifdef TEST_TURNBACK_FWD
		speed_Enable_Hbridge(false);
#endif
		if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
		{
			speed_set(MOTOR_RIGHT, avrSpeedRight);
			speed_set(MOTOR_LEFT, avrSpeedLeft);
		}
		else
		{
			currentDir=(currentDir+3)%4;
			CtrlStep++;
		}
		break;
	}
	case 5:
		posLeftTmp=qei_getPosLeft();
		posRightTmp=qei_getPosRight();
		CtrlStep++;
	case 6://turning another 90 degree
	{
#ifdef TEST_TURNBACK_TURN1
		speed_Enable_Hbridge(false);
#endif

		if ((abs(qei_getPosLeft()-posLeftTmp)+abs(qei_getPosRight()-posRightTmp))<turnPulse)
		{
			speed_set(MOTOR_RIGHT, -avrSpeedLeft);
			speed_set(MOTOR_LEFT, -avrSpeedRight);
		}
		else
		{
#ifdef TEST_TURNBACK_TURN2
		speed_Enable_Hbridge(false);
#endif
			currentDir=(currentDir+3)%4;
			clearPosition();
			qei_setPosLeft(resetEnc);
			qei_setPosRight(resetEnc);
			forwardUpdate();
			CtrlStep=1;
			return true;
		}
		break;
	}
	}
	return false;
}
Пример #7
0
static bool TurnRight(int fwdPulse,int avrSpeedLeft,int avrSpeedRight,int turnPulse,
		int resetEnc)
{
	static int vt,vp;
	LED1_OFF();LED2_OFF();LED3_ON();
	switch (CtrlStep)
	{
	case 1:
		posLeftTmp=qei_getPosLeft();
		CtrlStep=2;
		vt=1;
		vp=1;
		avrSpeedTmp=avrSpeed;
	case 2://go straight

		if ((abs(qei_getPosLeft()-posLeftTmp)<fwdPulse) ||
				(isWallFrontLeft && isWallFrontRight &&
				(IR_GetIrDetectorValue(3)>IR_get_calib_value(IR_CALIB_BASE_FRONT_RIGHT))&&
				(IR_GetIrDetectorValue(0)>IR_get_calib_value(IR_CALIB_BASE_FRONT_LEFT))))
		{
			if (qei_getPosLeft()<fwdPulse+posLeftTmp)
				avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
				+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
			else
				avrSpeed = (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
			if (isWallLeft)
				pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_LEFT);
			else
			{
				speed_set(MOTOR_RIGHT, avrSpeed);
				speed_set(MOTOR_LEFT, avrSpeed);
			}
		}
		else
		{
#ifdef TEST_TURNRIGHT_MOVE1
		speed_Enable_Hbridge(false);
#endif
			pid_reset(&pid_wall_left);
			pid_reset(&pid_wall_right);
			forwardUpdate();
			CtrlStep++;
			avrSpeed=avrSpeedTmp;
		}
		break;
	case 3:
		posLeftTmp=qei_getPosLeft();
		posRightTmp=qei_getPosRight();
		CtrlStep++;
	case 4://turn 90 degree
		if (abs(qei_getPosLeft()-posLeftTmp) + abs(qei_getPosRight()-posRightTmp) < turnPulse)
		{
			speed_set(MOTOR_LEFT, avrSpeedLeft);
			speed_set(MOTOR_RIGHT, -avrSpeedRight);
			if((abs(qei_getPosLeft()-posLeftTmp)>(turnPulse*0.8*vp/8)) && (vp<9))
			{
				if (avrSpeedLeft>=24)
					avrSpeedLeft-=24;
				vp++;
			}
			if((abs(qei_getPosRight()-posRightTmp)>(turnPulse*0.2*vt/8)) && (vt<9))
			{
				if (avrSpeedRight>=4)
					avrSpeedRight-=4;
				vt++;
			}
		}
		else
		{
#ifdef TEST_TURNRIGHT_TURN
		speed_Enable_Hbridge(false);
#endif
			currentDir=(currentDir+1)%4;
			clearPosition();
			qei_setPosLeft(resetEnc);
			qei_setPosRight(resetEnc);
			forwardUpdate();
			CtrlStep=1;
			pid_reset(&pid_wall_right);
			pid_reset(&pid_wall_left);
			speed_set(MOTOR_LEFT, avrSpeed);
			speed_set(MOTOR_RIGHT, avrSpeed);
			return true;
		}
		break;
	}
	return false;
}
Пример #8
0
//------------------------------------------------------------------------------
/// Application entry point. Configures the DBGU, PIT, TC0, LEDs and buttons
/// and makes LED\#1 blink in its infinite loop, using the Wait function.
/// \return Unused (ANSI-C compatibility).
//------------------------------------------------------------------------------
int main(void)
{


  // DBGU configuration
  TRACE_CONFIGURE(DBGU_STANDARD, 115200, BOARD_MCK);
  TRACE_INFO_WP("\n\r");
  TRACE_INFO("Getting new Started Project --\n\r");
  TRACE_INFO("%s\n\r", BOARD_NAME);
  TRACE_INFO("Compiled: %s %s --\n\r", __DATE__, __TIME__);

  //Configure Reset Controller
  AT91C_BASE_RSTC->RSTC_RMR= 0xa5<<24;

  // Configure EMAC PINS
  PIO_Configure(emacRstPins, PIO_LISTSIZE(emacRstPins));

  // Execute reset
  RSTC_SetExtResetLength(0xd);
  RSTC_ExtReset();

  // Wait for end hardware reset
  while (!RSTC_GetNrstLevel());

  TRACE_INFO("init Flash\n\r");
  flash_init();

  TRACE_INFO("init Timer\n\r");
  // Configure timer 0
  ticks=0;
  extern void ISR_Timer0();
  AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
  AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
  AT91C_BASE_TC0->TC_IDR = 0xFFFFFFFF;
  AT91C_BASE_TC0->TC_SR;
  AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV5_CLOCK | AT91C_TC_CPCTRG;
  AT91C_BASE_TC0->TC_RC = 375;
  AT91C_BASE_TC0->TC_IER = AT91C_TC_CPCS;
  AIC_ConfigureIT(AT91C_ID_TC0, AT91C_AIC_PRIOR_LOWEST, ISR_Timer0);
  AIC_EnableIT(AT91C_ID_TC0);
  AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;

  // Configure timer 1
  extern void ISR_Timer1();
  AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
  AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;	//Stop clock
  AT91C_BASE_TC1->TC_IDR = 0xFFFFFFFF;		//Disable Interrupts
  AT91C_BASE_TC1->TC_SR;						//Read Status register
  AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV4_CLOCK | AT91C_TC_CPCTRG;  // Timer1: 2,666us = 48MHz/128
  AT91C_BASE_TC1->TC_RC = 0xffff;
  AT91C_BASE_TC1->TC_IER = AT91C_TC_CPCS;
  AIC_ConfigureIT(AT91C_ID_TC1, 1, ISR_Timer1);
  AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;

  led_init();

  TRACE_INFO("init EEprom\n\r");
  eeprom_init();

  rb_reset(&TTY_Rx_Buffer);
  rb_reset(&TTY_Tx_Buffer);

  input_handle_func = analyze_ttydata;

  LED_OFF();
  LED2_OFF();
  LED3_OFF();

  spi_init();
  fht_init();
  tx_init();

  #ifdef HAS_ETHERNET

  ethernet_init();

  #endif

  TRACE_INFO("init USB\n\r");
  CDCDSerialDriver_Initialize();
  USBD_Connect();

  wdt_enable(WDTO_2S);

  fastrf_on=0;

  display_channel = DISPLAY_USB;

  TRACE_INFO("init Complete\n\r");

  checkFrequency();

  // Main loop
  while (1) {

    CDC_Task();
    Minute_Task();
    RfAnalyze_Task();

    #ifdef HAS_FASTRF
      FastRF_Task();
    #endif
    #ifdef HAS_RF_ROUTER
      rf_router_task();
    #endif
    #ifdef HAS_ASKSIN
      rf_asksin_task();
    #endif
    #ifdef HAS_MORITZ
      rf_moritz_task();
    #endif
    #ifdef HAS_RWE
      rf_rwe_task();
    #endif
    #ifdef HAS_MBUS
      rf_mbus_task();
    #endif
    #ifdef HAS_MAICO
      rf_maico_task();
    #endif

    #ifdef HAS_ETHERNET
      Ethernet_Task();
    #endif

#ifdef DBGU_UNIT_IN
    if(DBGU_IsRxReady()){
      unsigned char volatile * const ram = (unsigned char *) AT91C_ISRAM;
      unsigned char x;

      x=DBGU_GetChar();
      switch(x) {

      case 'd':
        puts("USB disconnect\n\r");
        USBD_Disconnect();
        break;
      case 'c':
        USBD_Connect();
        puts("USB Connect\n\r");
        break;
      case 'r':
        //Configure Reset Controller
        AT91C_BASE_RSTC->RSTC_RMR=AT91C_RSTC_URSTEN | 0xa5<<24;
        break;
      case 'S':
        USBD_Disconnect();

        my_delay_ms(250);
        my_delay_ms(250);

        //Reset
        *ram = 0xaa;
        AT91C_BASE_RSTC->RSTC_RCR = AT91C_RSTC_PROCRST | AT91C_RSTC_PERRST | AT91C_RSTC_EXTRST   | 0xA5<<24;
        while (1);
        break;
      default:
        rb_put(&TTY_Tx_Buffer, x);
      }
    }
#endif

    if (USBD_GetState() == USBD_STATE_CONFIGURED) {
      if( USBState == STATE_IDLE ) {
        CDCDSerialDriver_Read(usbBuffer,
                              DATABUFFERSIZE,
                              (TransferCallback) UsbDataReceived,
                              0);
        LED3_ON();
        USBState=STATE_RX;
      }
    }
    if( USBState == STATE_SUSPEND ) {
      TRACE_INFO("suspend  !\n\r");
      USBState = STATE_IDLE;
    }
    if( USBState == STATE_RESUME ) {
      TRACE_INFO("resume !\n\r");
      USBState = STATE_IDLE;
    }

  }
}
Пример #9
0
//------------------------------------------------------------------------------
//         Callbacks re-implementation
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
/// Invoked when the USB device leaves the Suspended state. By default,
/// configures the LEDs.
//------------------------------------------------------------------------------
void USBDCallbacks_Resumed(void)
{
  LED3_ON();
  USBState = STATE_RESUME;
}