Exemple #1
0
uint8_t LF_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"line help")==0) {
    LF_PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"line status")==0) {
    LF_PrintStatus(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"line start")==0) {
    LF_StartFollowing();
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)"line stop")==0) {
    LF_StopFollowing();
    *handled = TRUE;
  }
  return res;
}
static void StateMachine(bool buttonPress) {
  static uint8_t cnt;
  
  switch (appState) {
    case APP_STATE_INIT:
      LEDG_Off();
      LEDR_On();
      if (buttonPress) {
        APP_StateStartCalibrate(); 
      }
      break;
    case APP_STATE_CALIBRATE:
      cnt++;
      if (cnt>50) {
        cnt = 0;
        LEDR_Neg();
      }
      if (buttonPress) {
        APP_StateStopCalibrate(); 
      }
      break;
    case APP_STATE_IDLE:
      LEDR_Off();
      LEDG_On();
      if (buttonPress) {
        LF_StartFollowing();
        appState = APP_STATE_FOLLOW;
      }
      break;
    case APP_STATE_FOLLOW:
      LEDR_Off();
      LEDG_Off();
      if (!LF_IsFollowing()) {
        appState = APP_STATE_IDLE;
      }
      if (buttonPress) {
        LF_StopFollowing(); 
        appState = APP_STATE_IDLE;
      }
      break;
  } /* switch */
}
Exemple #3
0
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;
}
Exemple #4
0
static void StateMachine(bool buttonPress) {
#if PL_APP_FOLLOW_OBSTACLE || PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
  static uint8_t cnt;
#endif
  
  switch (appState) {
    case APP_STATE_INIT:
      LEDG_Off();
      LEDR_On();
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
      if (buttonPress) {
        APP_StateStartCalibrate(); 
      }
#elif PL_APP_FOLLOW_OBSTACLE
      appState = APP_STATE_IDLE;
#endif
      break;
#if PL_APP_FOLLOW_OBSTACLE
    case APP_STATE_FOLLOW_OBSTACLE:
      cnt++;
      if (cnt>50) {
        cnt = 0;
        LEDR_Neg();
      }
      if (buttonPress) {
        followObstacle = FALSE;
        appState = APP_STATE_IDLE;
      }
      break;
#endif
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
    case APP_STATE_CALIBRATE:
      cnt++;
      if (cnt>50) {
        cnt = 0;
        LEDR_Neg();
      }
      if (buttonPress) {
        APP_StateStopCalibrate(); 
      }
      break;
#endif
    case APP_STATE_IDLE:
      LEDR_Off();
      LEDG_On();
      if (buttonPress) {
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
        LF_StartFollowing();
        appState = APP_STATE_FOLLOW_LINE;
#elif PL_APP_FOLLOW_OBSTACLE
        followObstacle = TRUE;
        appState = APP_STATE_FOLLOW_OBSTACLE;
#endif
      }
      break;
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
    case APP_STATE_FOLLOW_LINE:
      LEDR_Off();
      LEDG_Off();
      if (!LF_IsFollowing()) {
        appState = APP_STATE_IDLE;
      }
      if (buttonPress) {
        LF_StopFollowing(); 
        appState = APP_STATE_IDLE;
      }
      break;
#endif
  } /* switch */
}
Exemple #5
0
static void REMOTE_HandleMotorMsg(int16_t direction, int16_t speedMode, int16_t z) {
  #define SCALE_DOWN 30
  #define MIN_VALUE  250 /* values below this value are ignored */
  #define DRIVE_DOWN 1

#define SCALE_FROM_PERCENT (16383/100)
#define NORMAL_SPEED (20 * SCALE_FROM_PERCENT)
#define FAST_SPEED (80 * SCALE_FROM_PERCENT)
#define FAST_TURN (30 * SCALE_FROM_PERCENT)

  if (!REMOTE_isOn) {
	  LED1_Off();
    return;
  }

  LED1_On();

  if(speedMode == 4) {
	  // left handed
	  LED2_On();
	  MAZE_SetTurnHandleLeft(TRUE);
	  LF_StartFollowing();
  }

  if(speedMode == 5) {
	  // right handed
	  LED2_On();
	  MAZE_SetTurnHandleLeft(FALSE);
	  LF_StartFollowing();
  }

  switch(direction) {
  case 0:											// straight
	  if(speedMode == 0){
		  DRV_SetSpeed(0, 0);
	  } else if (speedMode == 1) {
		  DRV_SetSpeed(NORMAL_SPEED, NORMAL_SPEED);
	  } else if (speedMode == 2) {
		  DRV_SetSpeed(FAST_SPEED,FAST_SPEED );
	  } else if (speedMode == 3){
		  DRV_SetSpeed( -NORMAL_SPEED, -NORMAL_SPEED );
	  }
	  break;

  case 1:											// right
	  if(speedMode == 0){
		  DRV_SetSpeed(NORMAL_SPEED, 0);
	  } else if (speedMode == 1) {
		  DRV_SetSpeed(FAST_SPEED, NORMAL_SPEED);
	  } else if (speedMode == 2) {
		  DRV_SetSpeed(FAST_SPEED,FAST_TURN );
	  } else if (speedMode == 3){
		  DRV_SetSpeed( -FAST_SPEED, -NORMAL_SPEED );
	  }
	  break;

  case 2:											// left
	  if(speedMode == 0){
		  DRV_SetSpeed(0, NORMAL_SPEED);
	  } else if (speedMode == 1) {
		  DRV_SetSpeed(NORMAL_SPEED, FAST_SPEED);
	  } else if (speedMode == 2) {
		  DRV_SetSpeed(FAST_TURN,FAST_SPEED );
	  } else if (speedMode == 3){
		  DRV_SetSpeed( -NORMAL_SPEED, -FAST_SPEED );
	  }
	  break;

  }
  /*
  if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down
#if PL_CONFIG_HAS_DRIVE
    DRV_SetSpeed(0, 0);
#else
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
  } else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
    int16_t speed, speedL, speedR;
    
    speed = speedVal/SCALE_DOWN;
    if (directionVal<0) {
      if (speed<0) {
        speedR = speed+(directionVal/SCALE_DOWN);
      } else {
        speedR = speed-(directionVal/SCALE_DOWN);
      }
      speedL = speed;
    } else {
      speedR = speed;
      if (speed<0) {
        speedL = speed-(directionVal/SCALE_DOWN);
      } else {
        speedL = speed+(directionVal/SCALE_DOWN);
      }
    }
#if PL_CONFIG_HAS_DRIVE
    DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN);
#else
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
  } else if (speedVal>100 || speedVal<-100) { //* speed
#if PL_CONFIG_HAS_DRIVE
    DRV_SetSpeed(speedVal, speedVal);
#else
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
  } else if (directionVal>100 || directionVal<-100) { //* direction
#if PL_CONFIG_HAS_DRIVE
    DRV_SetSpeed(directionVal/DRIVE_DOWN, -directionVal/DRIVE_DOWN);
#else
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
  } else { //* device flat on the table?
#if PL_CONFIG_HAS_DRIVE
    DRV_SetSpeed(0, 0);
#else
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
    MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif

  }  */
}
Exemple #6
0
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;
}