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 */
}
}
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;
}
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 */
}
}
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();
}
}
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;
}
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 */
}
