예제 #1
0
파일: QuadCalib.c 프로젝트: tbmathis/SUMO
static uint8_t Tune(const CLS1_StdIOType *io, uint8_t channel, MOT_MotorDevice *motorHandle) {
  #define TUNE_MOTOR_PERCENT 20
  uint16_t dac;
  int i;
  QuadTime_t timing;
  uint8_t buf[48];
  uint8_t res;
 
#if PL_HAS_DRIVE
  DRV_SetMode(DRV_MODE_NONE); /* turn off drive mode */
#endif
  MOT_SetSpeedPercent(motorHandle, TUNE_MOTOR_PERCENT);
  CLS1_SendStr((uint8_t*)"Tuning channel...\r\n", io->stdOut);
  res = ERR_FAILED;
  for(i=0,dac=0;dac<=MCP4728_MAX_DAC_VAL;i++) {
    UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
    UTIL1_chcat(buf, sizeof(buf), (uint8_t)('A'+channel)); /* 0:A, 1:B, 2:C, 3:D */
    UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
    UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
    UTIL1_chcat(buf, sizeof(buf), ' ');
    CLS1_SendStr(buf, io->stdOut);
    if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) { /* writes single channel DAC value, not updating EEPROM */
      CLS1_SendStr((uint8_t*)"ERROR writing DAC channel!\r\n", io->stdErr);
      res = ERR_FAILED;
      break;
    }
    WAIT1_WaitOSms(100); /* wait some time to allow DAC and OP-Amp change */
    if (Measure(channel, &timing)==ERR_OK) {
      buf[0] = '\0';
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
      CLS1_SendStr(buf, io->stdOut);
      if (timing.highPercent==50 || timing.lowPercent==50) {
        CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
        CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
        if (MCP4728_WriteDACandEE(channel, dac)!=ERR_OK) {
          CLS1_SendStr((uint8_t*)"ERROR writing DAC/EEPROM\r\n", io->stdErr);
          res = ERR_FAILED;
          break;
        }
        CLS1_SendStr((uint8_t*)"...done!\r\n", io->stdOut);
        res = ERR_OK;
        break; /* go to next channel */
      }
      dac += 0x1; /* smaller increase */
    } else {
      CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
      dac += 0x20; /* larger increase */
    }
  } /* for finding DAC value */
  MOT_SetSpeedPercent(motorHandle, 0); /* turn off again */
  if (res!=ERR_OK) {
    CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
  }
  CLS1_SendStr((uint8_t*)"Tuning finished!\r\n", io->stdOut);
  return res;
}
예제 #2
0
파일: Drive.c 프로젝트: sisem/intro
uint8_t DRV_Stop(int32_t timeoutMs) {
  DRV_SetMode(DRV_MODE_STOP); /* stop it */
  do {
    if (DRV_IsStopped()) {
      break;
    }
    WAIT1_WaitOSms(5);
    timeoutMs -= 5;
  } while (timeoutMs>0);
  if (timeoutMs<0) {
    return ERR_BUSY; /* timout */
  }
  return ERR_OK;
}
예제 #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;
}
예제 #4
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 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;
}
예제 #5
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;
}
예제 #6
0
void LF_StartFollowing(void) {
  PID_Start();
  LF_currState = STATE_FOLLOW_SEGMENT;
  DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
}
예제 #7
0
파일: Drive.c 프로젝트: sisem/intro
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  const unsigned char *p;
  int32_t val1, val2;

  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 ", sizeof("drive speed ")-1)==0) {
    p = cmd+sizeof("drive speed");
    if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
      if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
        if (DRV_SetSpeed(val1, val2)!=ERR_OK) {
          CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
        }
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
      }
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
    Q4CLeft_SetPos(0);
    Q4CRight_SetPos(0);
    if (DRV_SetPos(0, 0)!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
    }
    *handled = TRUE;
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) {
    p = cmd+sizeof("drive pos");
    if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
      if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
        if (DRV_SetPos(val1, val2)!=ERR_OK) {
          CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
        }
        *handled = TRUE;
      } else {
        CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
      }
    } else {
      CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
      res = ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) {
    p = cmd+sizeof("drive mode");
    if (UTIL1_strcmp((char*)p, (char*)"none")==0) {
      if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"stop")==0) {
      if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"speed")==0) {
      if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else if (UTIL1_strcmp((char*)p, (char*)"pos")==0) {
      if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) {
        res = ERR_FAILED;
      }
    } else {
      res = ERR_FAILED;
    }
    if (res!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
    }
    *handled = TRUE;
  }
  return res;
}