Exemple #1
0
void DRV_Init(void) {
  DRV_EnableDisable(FALSE);
  DRV_SpeedLeft = 0;
  DRV_SpeedRight = 0;

  DRV_EnableDisablePos(FALSE);
  DRV_PosLeft = 30000;
  DRV_PosRight = 30000;
  Q4CLeft_SetPos(30000);
  Q4CRight_SetPos(30000);


  if (FRTOS1_xTaskCreate(
        DriveTask,  /* pointer to the task */
        "Drive", /* task name for kernel awareness debugging */
        configMINIMAL_STACK_SIZE, /* task stack size */
        (void*)NULL, /* optional task startup argument */
        tskIDLE_PRIORITY,  /* initial priority */
        (xTaskHandle*)NULL /* optional task handle to create */
      ) != pdPASS) {
    /*lint -e527 */
    for(;;){} /* error! probably out of memory */
    /*lint +e527 */
  }

}
Exemple #2
0
static uint8_t HandleDataRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_HAS_SHELL
  uint8_t buf[32];
  CLS1_ConstStdIOTypePtr io = CLS1_GetStdio();
#endif
  uint8_t val;
  (void)size;
  (void)packet;
  switch(type) {
    case RAPP_MSG_TYPE_DATA: /* generic data message */
      *handled = TRUE;
      val = *data; /* get data value */
#if PL_HAS_DRIVE
      DRV_EnableDisable(TRUE);
#endif
#if PL_HAS_SHELL
      SHELL_SendString((unsigned char*)"Data: ");
      SHELL_SendString(data);
      SHELL_SendString((unsigned char*)" from addr 0x");
      buf[0] = '\0';
#if RNWK_SHORT_ADDR_SIZE==1
      UTIL1_strcatNum8Hex(buf, sizeof(buf), srcAddr);
#else
      UTIL1_strcatNum16Hex(buf, sizeof(buf), srcAddr);
#endif
      UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
      SHELL_SendString(buf);
#endif /* PL_HAS_SHELL */      
      return ERR_OK;
      break;
    default:
      break;
  } /* switch */
  return ERR_OK;
}
Exemple #3
0
void StateMachine_Init(void) {
  //refState = REF_STATE_INIT;
  //timerHandle = RefCnt_Init(NULL);
  /*! \todo You might need to adjust priority or other task settings */
  DRV_EnableDisable(0);
  if (FRTOS1_xTaskCreate(StateMachineTask, "StateMachine", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL) != pdPASS) {
    for(;;){} /* error */
  }
}
Exemple #4
0
static portTASK_FUNCTION(Fight_modus, pvParameters) {
#if PL_HAS_DRIVE
	  DRV_EnableDisable(FALSE);
	  DRV_EnableDisablePos(FALSE);
#endif
  fight_state = FIND_ENEMY;
  FRTOS1_vTaskDelay(100/TRG_TICKS_MS);
  for(;;) {
	  FRTOS1_vTaskDelay(10/TRG_TICKS_MS);
	  FightmodusV2();
  }
}
Exemple #5
0
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  const uint8_t *p;
  int32_t val32;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) {
    DRV_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) {
    DRV_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed L", sizeof("drive speed L")-1)==0) {
    p = cmd+sizeof("drive speed L");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_SpeedLeft = val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed R", sizeof("drive speed R")-1)==0) {
    p = cmd+sizeof("drive speed R");
    if (UTIL1_ScanDecimal32sNumber(&p, &val32)==ERR_OK) {
      DRV_SpeedRight = val32;
      *handled = TRUE;
    } else {
      res = ERR_FAILED;
    }
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive speed on")==0) {
    DRV_EnableDisable(TRUE);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"drive speed off")==0) {
    DRV_EnableDisable(FALSE);
    *handled = TRUE;
  }
  return res;
}
Exemple #6
0
static void StateMachine_Run(void){


	  int i;
	  int distance;

	  // Für Acceleration Sensor
	  int16_t x = MMA1_GetXmg();
	  WAIT1_Waitms(5);
	  int16_t y = MMA1_GetYmg();


	  switch (state) {
	    case START:
	      //SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");

	    		WAIT1_Waitms(4700);

	    		DRV_EnableDisable(1);
	    		DRV_SetSpeed(3000,3000);
	    		//WAIT1_Waitms(40);
	    		//DRV_SetSpeed(3000,3000);
	    		//state = ATTACK;

	    		//DRV_SetSpeed(-4200,4200);
	    	   // MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD);
	    		//MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD);
	    		//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30);
	    		//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30);
	    		//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);





	    		/*int x0;
	    		int x5;
	    		int x0_cal;
	    		int x5_cal;
	    		x0 = Get_Reflectance_Values(0);
	    		x0_cal = x0 / 15;
	    		x5 = Get_Reflectance_Values(5);
	    		x5_cal = x5 / 15;
	    		MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal);
	    		MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal);
	    		*/


	    		//uint16_t* calib_values_pointer;
	    		//calib_values_pointer = &SensorTimeType;
	    		//calib_values_pointer = S1_GetVal();

	    		state = DRIVE;

	      break;


	    case DRIVE:

	    	DRV_SetSpeed(-4200,4200);

	    	//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
	    	//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);


	    	if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
	    	{
	    		state = TURN;

	    		break;
	    	}



	    	distance = US_usToCentimeters(US_Measure_us(),22);

	        if((distance <= 30) && (distance != 0)){

	    		    		state = ATTACK;
	    		    	}



/*
	    	if((x >= 2000) || (x <= 2000))

	    	{
	    		state = STOP;
	    	}


	    	if((y >= 2000) || (y <= 2000))

	    	{
	    		state = STOP;
	    	}
*/



	    	break;



	    case TURN:
	    	      //SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
	    	//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
	    	//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40);
	    	    	// ATTACK
	    	DRV_SetSpeed(-5000,-5000);

	    	WAIT1_Waitms(500);


/*
	    	MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
	    	MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
	    	*/
	    	DRV_SetSpeed(4200,-4200);

	    	WAIT1_Waitms(300);

	    	DRV_SetSpeed(5000,5000);

	    	WAIT1_Waitms(50);

	    	if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500))
	    		    	{
	    		    		state = DRIVE;
	    		    	}



	    	break;

	    case ATTACK:


	    	DRV_SetSpeed(5500,5500);
	    	//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80);
	        //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80);

	        if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
	        	    	{
	        	    		state = TURN;

	        	    	}


/*

	        if((x >= 2000) || (x <= -2000))

	        	    	{
	        	    		state = STOP;
	        	    	}


	        if((y>= 2000) || (y <= -2000))

	        	    	{
	        	    		state = STOP;
	        	    	}

*/


	   	    	break;

	    case STOP:


	    	DRV_SetSpeed(0,0);
	    	/*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
	    	MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/


/*

	    	if((x >= -300) && (x <= 300))
	    	{
	    		if((y >= -300) && (y <= 300))
	    		{
	    			    state = DRIVE;
	    		}
	    	}
	    	*/

	   	   	    	break;


	  } /* whitch */
	}