uint8_t BUZ_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
const unsigned char *p;
uint16_t freq, duration;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"buzzer help")==0) {
*handled = TRUE;
return BUZ_PrintHelp(io);
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"buzzer status")==0) {
*handled = TRUE;
return BUZ_PrintStatus(io);
} else if (UTIL1_strncmp((char*)cmd, (char*)"buzzer buz ", sizeof("buzzer buz ")-1)==0) {
*handled = TRUE;
p = cmd+sizeof("buzzer buz ")-1;
if (UTIL1_ScanDecimal16uNumber(&p, &freq)==ERR_OK && UTIL1_ScanDecimal16uNumber(&p, &duration)==ERR_OK) {
if (BUZ_Beep(freq, duration)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"Starting buzzer failed\r\n", io->stdErr);
return ERR_FAILED;
}
return ERR_OK;
}
} else if (UTIL1_strcmp((char*)cmd, (char*)"buzzer play tune")==0) {
*handled = TRUE;
return BUZ_PlayTune();
}
return ERR_OK;
}
static void CheckButton(void) {
uint32_t timeTicks; /* time in ticks */
#define BUTTON_CNT_MS 100 /* iteration count for button */
bool autoCalibrate = FALSE;
if (SW1_GetVal()==0) { /* button pressed */
/* short press (1 beep): start or stop line following if calibrated
* 1 s press (2 beep): calibrate manually
* 2 s press (3 beep): calibrate with auto-move
* 3 s press (4 beep or more): clear path
* */
FRTOS1_vTaskDelay(50/portTICK_RATE_MS); /* simple debounce */
if (SW1_GetVal()==0) { /* still pressed */
LEDG_On();
timeTicks = 0;
while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) {
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
if ((timeTicks%(1000/BUTTON_CNT_MS))==0) {
#if PL_HAS_BUZZER
BUZ_Beep(300, 200);
#endif
}
timeTicks++;
} /* wait until released */
autoCalibrate = FALSE;
if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */
CLS1_SendStr((unsigned char*)"button press.\r\n", CLS1_GetStdio()->stdOut);
StateMachine(TRUE); /* <1 s, short button press, according to state machine */
} else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */
} else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"auto calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 2-3 s: start auto calibration */
autoCalibrate = TRUE;
} else if (timeTicks>=(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"delete solution.\r\n", CLS1_GetStdio()->stdOut);
MAZE_ClearSolution();
}
while (SW1_GetVal()==0) { /* wait until button is released */
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
}
if (autoCalibrate) {
CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", CLS1_GetStdio()->stdOut);
/* perform automatic calibration */
WAIT1_WaitOSms(1500); /* wait some time */
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_STOP);
APP_StateStopCalibrate();
CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", CLS1_GetStdio()->stdOut);
}
}
} /* if */
}
static void BUZ_Play(void *dataPtr) {
int idx = (int)dataPtr;
BUZ_Beep(Melody[idx].freq, Melody[idx].ms);
idx++;
if (idx<(sizeof(Melody)/sizeof(Melody[0]))) {
TRG_SetTrigger(TRG_BUZ_TUNE, Melody[idx-1].ms/TRG_TICKS_MS, BUZ_Play, (void*)idx);
}
}
static void BUZ_Play(void *dataPtr) {
MelodyDesc *melody = (MelodyDesc*)dataPtr;
BUZ_Beep(melody->melody[melody->idx].freq, melody->melody[melody->idx].ms);
melody->idx++;
if (melody->idx<melody->maxIdx) {
TRG_SetTrigger(TRG_BUZ_TUNE, melody->melody[melody->idx-1].ms/TRG_TICKS_MS, BUZ_Play, (void*)melody);
}
}
static portTASK_FUNCTION(ReflTask, pvParameters) {
(void)pvParameters; /* not used */
for(;;) {
if (doMinMaxCalibration) {
REF_CalibrateMinMax(SensorWhite, SensorBlack, SensorRaw);
BUZ_Beep(300, 50);
} else {
REF_Measure();
}
FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
}
}
uint8_t BUZ_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io)
{
if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "BUZ1 help")==0) {
*handled = TRUE;
CLS1_SendHelpStr((unsigned char*)"BUZ", (unsigned char*)"Group of BUZ commands\r\n", io->stdOut);
CLS1_SendHelpStr((unsigned char*)" beep", (unsigned char*)"Beep with 1kHz for 1 sec\r\n", io->stdOut);
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "BUZ beep")==0) {
*handled = TRUE;
return BUZ_Beep(1000, 1000);
}
return ERR_OK;
}
static void APP_HandleEvent(EVNT_Handle event) {
switch(event) {
case EVNT_INIT:
LED1_On(); WAIT1_Waitms(500); LED1_Off();
break;
#if PL_NOF_KEYS>=1
case EVNT_SW1_PRESSED:
LED1_Neg();
#if PL_HAS_BUZZER
(void)BUZ_Beep(300, 500);
#endif
#if PL_HAS_REMOTE
if (REMOTE_GetOnOff()) {
REMOTE_SetOnOff(FALSE);
} else {
REMOTE_SetOnOff(TRUE);
}
#endif
break;
#endif
#if PL_NOF_KEYS>=2
case EVNT_SW2_PRESSED:
LED2_Neg();
#if PL_HAS_REMOTE && PL_APP_ACCEL_CONTROL_SENDER
//(void)RSTDIO_SendToTxStdio(RSTDIO_QUEUE_TX_IN, "buzzer buz 800 400\r\n", sizeof("buzzer buz 800 400\r\n")-1);
#endif
break;
#endif
#if PL_NOF_KEYS>=3
case EVNT_SW3_PRESSED:
LED3_Neg();
break;
#endif
#if PL_NOF_KEYS>=4
case EVNT_SW4_PRESSED:
LED4_Neg();
break;
#endif
#if PL_HAS_LED_HEARTBEAT
case EVNT_LED_HEARTBEAT:
LED4_Neg();
break;
#endif
default:
#if PL_HAS_RADIO
//RADIO_AppHandleEvent(event);
#endif
break;
}
}
static void StateMachine(void) {
switch (LF_currState) {
case STATE_IDLE:
break;
case STATE_FOLLOW_SEGMENT:
if (!FollowSegment(LINE_FOLLOW_FW)) {
#if PL_APP_LINE_MAZE
LF_currState = STATE_TURN; /* make turn */
#else
LF_currState = STATE_STOP; /* stop if we do not have a line any more */
#endif
}
break;
#if PL_APP_LINE_MAZE
case STATE_FOLLOW_SEGMENT_BW:
if (!FollowSegment(FALSE)) {
TURN_Turn(TURN_STOP);
if (EvaluateTurnBw()==ERR_OK) {
LF_currState = STATE_FOLLOW_SEGMENT;
} else {
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_TURN:
if (MAZE_IsSolved()) {
TURN_Kind turn;
turn = MAZE_GetSolvedTurn(&LF_solvedIdx);
if (turn==TURN_STOP) { /* last turn reached */
TURN_Turn(turn);
LF_currState = STATE_FINISHED;
} else { /* perform turning */
TURN_Turn(TURN_STEP_LINE_FW); /* Step over line */
TURN_Turn(TURN_STEP_POST_LINE_FW); /* step before doing the turn */
TURN_Turn(turn);
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* still evaluating maze */
bool deadEndGoBw = FALSE;
bool finished = FALSE;
if (EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */
if (finished) {
LF_currState = STATE_FINISHED;
MAZE_SetSolved();
#if PL_TURN_ON_FINISH
/* turn the robot */
TURN_Turn(TURN_LEFT180);
#endif
TURN_Turn(TURN_STOP);
/* now ready to do line following */
} else if (deadEndGoBw) {
LF_currState = STATE_FOLLOW_SEGMENT_BW;
} else {
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* error case */
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_FINISHED:
#if PL_HAS_BUZZER
{
uint8_t i;
for(i=0;i<4;i++) {
(void)BUZ_Beep(300, 100);
WAIT1_WaitOSms(500);
}
}
#endif
LF_currState = STATE_STOP;
break;
#endif
case STATE_STOP:
TURN_Turn(TURN_STOP);
LF_currState = STATE_IDLE;
break;
} /* switch */
}
/*
* \brief this is a test function for the Trigger Option
*/
void TEST_onButton(void)
{
uint8_t err;
err = BUZ_Beep(500,1000);
}
void RTOS_HandleEvent(byte event) {
switch(event) {
#if PL_HAS_HW_NAV_SWITCH
case EVNT1_BUTTON1_RELEASED:
#if PL_HAS_HW_LCD /* rotate switch meaning depending on orientation */
switch(GDisp1_GetDisplayOrientation()) {
case GDisp1_ORIENTATION_LANDSCAPE180: EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT); break;
case GDisp1_ORIENTATION_PORTRAIT: EVNT1_SetEvent(EVNT1_BUTTON_UI_DOWN); break;
case GDisp1_ORIENTATION_LANDSCAPE: EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT); break;
case GDisp1_ORIENTATION_PORTRAIT180: EVNT1_SetEvent(EVNT1_BUTTON_UI_UP); break;
} /* switch */
#else
EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT);
#endif
break;
case EVNT1_BUTTON2_RELEASED:
#if PL_HAS_HW_LCD /* rotate switch meaning depending on orientation */
switch(GDisp1_GetDisplayOrientation()) {
case GDisp1_ORIENTATION_LANDSCAPE180: EVNT1_SetEvent(EVNT1_BUTTON_UI_UP); break;
case GDisp1_ORIENTATION_PORTRAIT: EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT); break;
case GDisp1_ORIENTATION_LANDSCAPE: EVNT1_SetEvent(EVNT1_BUTTON_UI_DOWN); break;
case GDisp1_ORIENTATION_PORTRAIT180: EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT); break;
} /* switch */
#else
EVNT1_SetEvent(EVNT1_BUTTON_UI_DOWN);
#endif
break;
case EVNT1_BUTTON5_RELEASED:
#if PL_HAS_HW_LCD /* rotate switch meaning depending on orientation */
switch(GDisp1_GetDisplayOrientation()) {
case GDisp1_ORIENTATION_LANDSCAPE180: EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT); break;
case GDisp1_ORIENTATION_PORTRAIT: EVNT1_SetEvent(EVNT1_BUTTON_UI_UP); break;
case GDisp1_ORIENTATION_LANDSCAPE: EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT); break;
case GDisp1_ORIENTATION_PORTRAIT180: EVNT1_SetEvent(EVNT1_BUTTON_UI_DOWN); break;
} /* switch */
#else
EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT);
#endif
break;
case EVNT1_BUTTON3_PRESSED:
EVNT1_SetEvent(EVNT1_BUTTON_UI_MIDDLE_DOWN);
break;
case EVNT1_BUTTON3_RELEASED:
EVNT1_SetEvent(EVNT1_BUTTON_UI_MIDDLE);
break;
case EVNT1_BUTTON4_RELEASED:
#if PL_HAS_HW_LCD /* rotate switch meaning depending on orientation */
switch(GDisp1_GetDisplayOrientation()) {
case GDisp1_ORIENTATION_LANDSCAPE180: EVNT1_SetEvent(EVNT1_BUTTON_UI_DOWN); break;
case GDisp1_ORIENTATION_PORTRAIT: EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT); break;
case GDisp1_ORIENTATION_LANDSCAPE: EVNT1_SetEvent(EVNT1_BUTTON_UI_UP); break;
case GDisp1_ORIENTATION_PORTRAIT180: EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT); break;
} /* switch */
#else
EVNT1_SetEvent(EVNT1_BUTTON_UI_UP);
#endif
break;
#elif (PL_BOARD_TWR_CN128 || PL_BOARD_TWR_52259) /* use the TWR SW2 and SW3, translate to navigation messages */
case EVNT1_BUTTON1_RELEASED: EVNT1_SetEvent(EVNT1_BUTTON_UI_LEFT); break;
case EVNT1_BUTTON2_RELEASED: EVNT1_SetEvent(EVNT1_BUTTON_UI_RIGHT); break;
case EVNT1_BUTTON1_RELEASED_LONG: EVNT1_SetEvent(EVNT1_BUTTON_UI_MIDDLE); break;
case EVNT1_BUTTON2_RELEASED_LONG: EVNT1_SetEvent(EVNT1_BUTTON_UI_MIDDLE); break;
#endif
case EVNT1_BUZZER_BEEP:
#if PL_HAS_HW_SOUNDER
BUZ_Beep();
#endif
break;
default:
break;
} /* switch */
/* call other event handler... */
#if PL_HAS_UI
APP_HandleEvent(event);
#endif
#if PL_APP_MODE_I2C_LCD
I2C_HandleEvent(event);
#endif
}
static void APP_EventHandler(EVNT_Handle event) {
#if PL_HAS_LINE_SENSOR
#endif
static uint16_t buzzer = 1000;
switch(event){
case EVNT_INIT:
LED1_On();
WAIT1_Waitms(100);
LED1_Off();
#if PL_HAS_BUZZER
BUZ_Beep(1000,400);
#endif
break;
case EVNT_HEARTBEAT:
//LED1_Off();
break;
case EVNT_SW_A_RELEASED:
//BUZ_Beep(2000,2000);
break;
case EVNT_SW_A_PRESSED:
#if PL_HAS_BUZZER
BUZ_Beep(1000,10);
#if PL_HAS_BATTLE
BATTLE_changeState(BATTLE_STATE_WAIT);
#endif /* PL_HAS_BATTLE */
#endif
break;
case EVNT_SW_A_LPRESSED:
#if PL_HAS_LINE_SENSOR
LED1_Neg();
EVNT_SetEvent(EVNT_REF_START_STOP_CALIBRATION);
#endif
break;
case EVNT_SW_B_PRESSED:
LED2_Neg();
LED3_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW B pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_SW_C_PRESSED:
LED1_Neg();
LED2_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW C pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_SW_D_PRESSED:
LED1_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW D pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_SW_E_PRESSED:
LED2_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW E pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_SW_F_PRESSED:
LED3_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW F pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_SW_G_PRESSED:
LED1_Neg();
LED2_Neg();
LED3_Neg();
#if PL_HAS_SHELL
CLS1_SendStr("SW g pressed\n",CLS1_GetStdio()->stdOut);
#endif
break;
case EVNT_REF_START_STOP_CALIBRATION:
break;
default:
break;
}
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-15 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button - disable remote, drive mode none */
DRV_SetMode(DRV_MODE_NONE);
REMOTE_SetOnOff(FALSE);
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
REMOTE_SetOnOff(TRUE);
#if PL_CONFIG_HAS_DRIVE
DRV_SetMode(DRV_MODE_SPEED);
#endif
SHELL_SendString("Remote ON\r\n");
}
#if PL_CONFIG_HAS_LINE_MAZE
else if (val=='A') { /* green 'A' button */
SHELL_SendString("Button A pressed\r\n");
// Start Maze solving
if(!LF_IsFollowing()){
LF_StartFollowing();
}
} else if (val=='K') { /* green A button longpress -> 'K' */
SHELL_SendString("Clear Maze\r\n");
// Clear old maze solution, ready for restart
MAZE_ClearSolution();
} else if (val =='B'){ /* yellow 'B' button */
SHELL_SendString("Right hand rule!\r\n");
LF_SetRule(FALSE);
} else if (val=='E') { /* button 'E' pressed */
SHELL_SendString("Stop Following! \r\n");
if(LF_IsFollowing()){
LF_StopFollowing();
}
} else if (val=='D') { /* blue 'D' button */
SHELL_SendString("Left hand rule!\r\n");
LF_SetRule(TRUE);
}
#endif
else if (val=='C') { /* red 'C' button */
NITRO = TRUE;
SHELL_SendString("Nitrooooooo!!!\r\n");
BUZ_Beep(1000,1000);
} else if (val=='J') { /* button 'C' released */
NITRO = FALSE;
SHELL_SendString("Stop Nitro \r\n");
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
static void StateMachine_Run(void){
int i;
int distance;
switch (state) {
case START:
WAIT1_WaitOSms(800);
BUZ_Beep(400,400);
WAIT1_WaitOSms(800);
BUZ_Beep(450,400);
WAIT1_WaitOSms(800);
BUZ_Beep(500,400);
WAIT1_WaitOSms(800);
BUZ_Beep(600,400);
WAIT1_WaitOSms(800);
BUZ_Beep(880,400);
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), 40);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 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:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_TURN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -SPEED_TURN);
if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz
state = STOP;
}
distance = US_usToCentimeters(US_Measure_us(),22);
if((distance <= 30) && (distance != 0)){
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK);
state = ATTACK;
}
/*if((MMA1_GetXmg() >= 500) || (MMA1_GetXmg() <= -500)){
state = STOP;
}
*/
/*if((MMA1_GetYmg() >= 500) || (MMA1_GetYmg() <= -500)){
state = STOP;
}*/
break;
case TURN:
state = DRIVE;
break;
case STOP:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -50);
WAIT1_WaitOSms(150);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 50);
WAIT1_WaitOSms(444);
if(Get_Reflectance_Values(4) >= 400){
state = DRIVE;
}
/*if((MMA1_GetXmg() >= -200) && (MMA1_GetXmg() <= 200)){
state = DRIVE;
}*/
/*if((MMA1_GetYmg() >= -200) || (MMA1_GetYmg() <= 200)){
state = DRIVE;
}*/
break;
case ATTACK:
if((distance <= 30) && (distance != 0)){
state = ATTACK;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK);
WAIT1_WaitOSms(50);
}else{
state = DRIVE;
}
if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz
state = STOP;
}
break;
} /* switch */
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" IR led on", ledON?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrating", doMinMaxCalibration?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrated", isCalibrated?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min noise", buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_LINE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min line", buf, io->stdOut);
CLS1_SendStatusStr((unsigned char*)" raw val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" min val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMin[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" max val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMax[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calib val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" line val", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#if PL_APP_LINE_MAZE
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
CLS1_SendStatusStr((unsigned char*)" history", (unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorHistory[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#endif
CLS1_SendStatusStr((unsigned char*)" line kind", REF_LineKindStr(refLineKind), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
byte REF_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "ref help")==0) {
*handled = TRUE;
return PrintHelp(io);
} else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "ref status")==0)) {
*handled = TRUE;
return PrintStatus(io);
} else if (UTIL1_strcmp((char*)cmd, "ref calib on")==0) {
APP_StateStartCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref calib off")==0) {
APP_StateStopCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led on")==0) {
ledON = TRUE;
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led off")==0) {
ledON = FALSE;
*handled = TRUE;
return ERR_OK;
}
return ERR_OK;
}
uint16_t REF_GetLineValue(bool *onLine) {
*onLine = refCenterLineVal>0 && refCenterLineVal<REF_MAX_LINE_VALUE;
return refCenterLineVal;
}
static portTASK_FUNCTION(ReflTask, pvParameters) {
(void)pvParameters; /* not used */
for(;;) {
if (doMinMaxCalibration) {
REF_CalibrateMinMax(SensorMin, SensorMax, SensorRaw);
#if PL_HAS_BUZZER
BUZ_Beep(300, 50);
#endif
} else {
REF_Measure();
}
FRTOS1_vTaskDelay(10/portTICK_RATE_MS);
}
}
void REF_Init(void) {
refLineKind = REF_LINE_NONE;
refCenterLineVal = 0;
mutexHandle = FRTOS1_xSemaphoreCreateMutex();
if (mutexHandle==NULL) {
for(;;);
}
timerHandle = RefCnt_Init(NULL);
REF_InitSensorValues();
if (FRTOS1_xTaskCreate(ReflTask, (signed portCHAR *)"Refl", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+1+2, NULL) != pdPASS) {
for(;;){} /* error */
}
}
static void APP_EventHandler(EVNT_Handle event) {
switch(event) {
case EVNT_INIT:
/*LED1_On();
WAIT1_Waitms(50);
LED1_Off();
LED2_On();
WAIT1_Waitms(50);
LED2_Off();
LED3_On();
WAIT1_Waitms(50);
LED3_Off();*/
break;
case EVENT_LED_HEARTBEAT:
//LED1_Neg();
break;
#if PL_NOF_KEYS >= 1
case EVNT_SW1_PRESSED:
lastKeyPressed = 1;
#if PL_HAS_SHELL
SHELL_SendString("SW1 pressed!\r\n");
#endif
#if PL_HAS_BUZZER
BUZ_Beep(300, 500);
#endif
break;
case EVNT_SW1_LPRESSED:
#if PL_HAS_SHELL
SHELL_SendString("SW1 long pressed!\r\n");
#endif
break;
case EVNT_SW1_RELEASED:
#if PL_HAS_SHELL
SHELL_SendString("SW1 released!\r\n");
#endif
break;
#endif
#if PL_NOF_LEDS_ROBO
case EVNT_SW2_PRESSED:
lastKeyPressed = 2;
#if PL_HAS_SHELL
SHELL_SendString("SW2 pressed!\r\n");
EVNT_SetEvent(EVNT_REF_START_STOP_CALIBRATION);
#endif
break;
case EVNT_SW2_LPRESSED:
LED_U1_Neg();
#if PL_HAS_SHELL
SHELL_SendString("SW2 long pressed!\r\n");
#endif
break;
case EVNT_SW2_RELEASED:
LED_U1_Neg();
#if PL_HAS_SHELL
SHELL_SendString("SW2 released!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 3
case EVNT_SW3_PRESSED:
lastKeyPressed = 3;
#if PL_HAS_SHELL
SHELL_SendString("SW3 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 4
case EVNT_SW4_PRESSED:
lastKeyPressed = 4;
#if PL_HAS_SHELL
SHELL_SendString("SW4 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 5
case EVNT_SW5_PRESSED:
lastKeyPressed = 5;
#if PL_HAS_SHELL
SHELL_SendString("SW5 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 6
case EVNT_SW6_PRESSED:
lastKeyPressed = 6;
#if PL_HAS_SHELL
SHELL_SendString("SW6 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 7
case EVNT_SW7_PRESSED:
lastKeyPressed = 7;
#if PL_HAS_SHELL
SHELL_SendString("SW7 pressed!\r\n");
#endif
break;
#endif
default:
break;
}
}
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
#if NVM_ENABLED
refState = REF_GetCalibData();
#else
refState = REF_STATE_NOT_CALIBRATED;
SHELL_SendString("INFO: Sensor not calibrated");
#endif
break;
case REF_STATE_NOT_CALIBRATED:
REF_MeasureRaw(SensorRaw);
/*! \todo Add a new event to your event module...*/
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
case REF_STATE_START_CALIBRATION:
SHELL_SendString((unsigned char*)"start calibration...\r\n");
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMax.minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMax.maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
break;
case REF_STATE_CALIBRATING:
REF_CalibrateMinMax(SensorCalibMinMax.minVal, SensorCalibMinMax.maxVal, SensorRaw);
#if PL_CONFIG_HAS_BUZZER
(void)BUZ_Beep(300, 20);
#endif
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_STOP_CALIBRATION;
}
#endif
break;
case REF_STATE_STOP_CALIBRATION:
SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
#if NVM_ENABLED
SHELL_SendString((unsigned char*)"saving calib data...\r\n");
REF_SaveCalibData();
#endif
refState = REF_STATE_READY;
break;
case REF_STATE_READY:
REF_Measure();
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
} /* switch */
}
