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>-5 && 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, toggle remote*/ SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500"); DRV_SetMode(DRV_MODE_SPEED); } else if (val=='G') { /* center joystick button: horn*/ SHELL_ParseCmd((unsigned char*)"buzzer buz 2000 300"); } else if (val=='A') { MAZE_ClearSolution(); MAZE_SetSolveAlgorithm(STRAIGHT_HAND); LF_StartFollowing(); } else if (val=='C') { /* 'C' button: motor stop*/ DRV_SetMode(DRV_MODE_STOP); } else if (val=='B') { /* 'B' button: start right-hand algorithm */ MAZE_ClearSolution(); MAZE_SetSolveAlgorithm(RIGHT_HAND); LF_StartFollowing(); } else if (val=='D') { /* 'D' button: start left-hand algorithm */ MAZE_ClearSolution(); MAZE_SetSolveAlgorithm(LEFT_HAND); LF_StartFollowing(); } else if (val=='E') { REF_CalibrateStartStop(); } #else *handled = FALSE; /* no shell and no buzzer? */ #endif break; default: break; } /* switch */ return ERR_OK; }
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 direction, speed; int16_t x1000, y1000; *handled = TRUE; direction = *data; /* get x data value */ speed = *(data+1); /* get y data value */ if (REMOTE_isVerbose) { UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: "); UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction); UTIL1_chcat(buf, sizeof(buf), ','); UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed); 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 //x1000 = scaleJoystickTo1K(x); //y1000 = scaleJoystickTo1K(y); if (REMOTE_useJoystick) { REMOTE_HandleMotorMsg(direction, speed, 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 */ SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500"); REMOTE_SetOnOff(FALSE); DRV_SetSpeed(0,0); /* turn off motors */ SHELL_SendString("Remote OFF\r\n"); } else if (val=='G') { /* center joystick button: enable remote */ SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000"); REMOTE_SetOnOff(TRUE); DRV_SetMode(DRV_MODE_SPEED); SHELL_SendString("Remote ON\r\n"); } else if (val=='C') { /* red 'C' button */ /*! \todo add functionality */ } else if (val=='A') { /* green 'A' button */ /*! \todo add functionality */ } #else *handled = FALSE; /* no shell and no buzzer? */ #endif break; default: break; } /* switch */ return ERR_OK; }
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 KEY_DATA 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>-5 && 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 switch(val) { case A_p: SHELL_ParseCmd((unsigned char*)"buzzer play tune"); break; case A_lp: nitro = TRUE; break; case A_r: nitro = FALSE; break; case B_p: SHELL_ParseCmd((unsigned char*)"line start"); break; case B_lp: break; case B_r: break; case C_p: break; case C_lp: break; case C_r: break; case D_p: SHELL_ParseCmd((unsigned char*)"line stop"); SHELL_ParseCmd((unsigned char*)"drive mode speed"); break; case D_lp: break; case D_r: break; case E_p: SHELL_ParseCmd((unsigned char*)"line righthand"); break; case E_lp: break; case E_r: break; case F_p: SHELL_ParseCmd((unsigned char*)"line lefthand"); break; case F_lp: break; case F_r: break; case G_p: break; case G_lp: break; case G_r: break; default: SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000"); break; } #else *handled = FALSE; /* no shell and no buzzer? */ #endif break; default: break; } /* switch */ return ERR_OK; }
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; }
/*! * \brief Performs a turn. * \return Returns TRUE while turn is still in progress. */ uint8_t MAZE_EvaluteTurn(bool *finished) { REF_LineKind historyLineKind, currLineKind; TURN_Kind turn; *finished = FALSE; if(MAZE_IsSolved()){ TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, MAZE_SampleTurnStopFunction); /* do the line and beyond in one step */ SHELL_ParseCmd((unsigned char*)"drive mode none"); turn = MAZE_GetSolvedTurn(&indexPath); } else{ currLineKind = REF_GetLineKind(); if (currLineKind==REF_LINE_NONE) { /* nothing, must be dead end */ turn = TURN_LEFT180; } else { MAZE_ClearSensorHistory(); /* clear history values */ MAZE_SampleSensorHistory(); /* store current values */ TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, MAZE_SampleTurnStopFunction); /* do the line and beyond in one step */ historyLineKind = MAZE_HistoryLineKind(); /* new read new values */ currLineKind = REF_GetLineKind(); turn = MAZE_SelectTurn(historyLineKind, currLineKind); } if(!isSolved){ MAZE_AddPath(turn); } } if (turn==TURN_FINISHED) { *finished = TRUE; LF_StopFollowing(); SHELL_SendString((unsigned char*)"MAZE: finished!\r\n"); return ERR_OK; } else if (turn==TURN_STRAIGHT) { SHELL_ParseCmd((unsigned char*)"drive mode none"); //SHELL_SendString((unsigned char*)"going straight\r\n"); return ERR_OK; } else if (turn==TURN_STOP) { /* should not happen here? */ LF_StopFollowing(); SHELL_SendString((unsigned char*)"Failure, stopped!!!\r\n"); return ERR_FAILED; /* error case */ } else if (turn==TURN_LEFT180){ TURN_Turn(TURN_LEFT180, NULL); SHELL_ParseCmd((unsigned char*)"drive mode none"); } else if (turn==TURN_RIGHT90){ TURN_Turn(TURN_RIGHT90, NULL); SHELL_ParseCmd((unsigned char*)"drive mode none"); } else if (turn==TURN_LEFT90){ TURN_Turn(TURN_LEFT90, NULL); SHELL_ParseCmd((unsigned char*)"drive mode none"); }else if (turn==TURN_FINISHED){ SHELL_ParseCmd((unsigned char*)"drive mode none"); LF_StopFollowing(); }else { /* turn or do something */ /*! \todo Extend if necessary */ return ERR_OK; /* turn finished */ } }