Esempio n. 1
0
/*
** ===================================================================
**     Method      :  CLS1_PrintPrompt (component Shell)
**     Description :
**         Prints the prompt to the stdOut channel
**     Parameters  :
**         NAME            - DESCRIPTION
**       * io              - Pointer to IO to be used
**     Returns     : Nothing
** ===================================================================
*/
void CLS1_PrintPrompt(CLS1_ConstStdIOType *io)
{
  CLS1_SendStr((unsigned char*)"CMD> ", io->stdOut);
}
Esempio n. 2
0
/*
** ===================================================================
**     Method      :  CLS1_ReadLine (component Shell)
**     Description :
**         Reads a line from stdIn and returns TRUE if we have a line,
**         FALSE otherwise.
**     Parameters  :
**         NAME            - DESCRIPTION
**       * bufStart        - Pointer to start of buffer
**       * buf             - Pointer to buffer where to read in the
**                           information
**         bufSize         - size of buffer
**       * io              - Pointer to I/O callbacks
**     Returns     :
**         ---             - Error code
** ===================================================================
*/
bool CLS1_ReadLine(uint8_t *bufStart, uint8_t *buf, size_t bufSize, CLS1_ConstStdIOType *io)
{
  uint8_t c;

  if (io->keyPressed()) {
    for(;;) {                          /* while not '\r' or '\n' */
      c = '\0';                        /* initialize character */
      io->stdIn(&c);                   /* read character */
      if (c=='\0') { /* nothing in rx buffer? Something is wrong... */
        break; /* get out of loop */
      }
      if (c=='\b' || c=='\177') {      /* check for backspace */
        if (buf > bufStart) {          /* Avoid buffer underflow */
#if CLS1_ECHO_ENABLED
           io->stdOut('\b');           /* delete character on terminal */
           io->stdOut(' ');
           io->stdOut('\b');
#endif
           buf--;                      /* delete last character in buffer */
           *buf = '\0';
           bufSize++;
        }
      } else if
        (
            isalnum(c) || (c==' ') || (c=='\r') || (c=='\n')
        #if CLS1_SILENT_PREFIX_CHAR_ENABLED
          || (c==CLS1_SILENT_PREFIX_CHAR)
        #endif
        #if CLS1_MULTI_CMD_ENABLED
          || (c==CLS1_MULTI_CMD_CHAR)
        #endif
        )
      {
#if CLS1_ECHO_ENABLED
        io->stdOut(c);                 /* echo character */
#endif
        *buf = (uint8_t)c;             /* append character to the string */
        buf++;
        bufSize--;
        if ((c=='\r') || (c=='\n')) {
#if CLS1_ECHO_ENABLED
          CLS1_SendStr((unsigned char*)"\n", io->stdOut);
#endif
#if CLS1_HISTORY_ENABLED
          if ((bufStart[0] != '\0') && (bufStart[0] != '\r') && (bufStart[0] != '\n')) {
            int i;

            for(i=CLS1_NOF_HISTORY-1; i>0;i--) {
              UTIL1_strcpy(CLS1_history[i], CLS1_HIST_LEN, CLS1_history[i-1]); /* move previous commands */
            }
            CLS1_history_index = 0;    /* update the history with the current command */
            UTIL1_strcpy(CLS1_history[0], CLS1_HIST_LEN, bufStart); /* add the current command to the history */
            if (buf-bufStart <= CLS1_HIST_LEN) { /* size check */
              CLS1_history[0][buf-bufStart-1] = '\0';
            } else {
              CLS1_history[0][CLS1_HIST_LEN-1] = '\0';
            }
          }
#endif
          break;
        }
        if (bufSize <= 1) {            /* buffer full */
          break;
        }
#if CLS1_SILENT_PREFIX_CHAR_ENABLED
      } else if (c!=CLS1_SILENT_PREFIX_CHAR) { /* silent prefix enabled, might be non-alphanumeric */
#else
      } else {                         /* not an alphanumeric */
#endif
#if CLS1_HISTORY_ENABLED
        uint8_t cBuf[3]={'\0','\0','\0'}, cBufIdx = 0;
        bool prevInHistory;
#endif

        while (c!='\0') {              /* empty the rx buffer (escape sequence) */
#if CLS1_HISTORY_ENABLED
           cBuf[cBufIdx] = c;
           cBufIdx++;
           if (cBufIdx==sizeof(cBuf)) {
             cBufIdx = 0; /* ring buffer */
           }
#endif
           c = '\0';                   /* initialize character */
           io->stdIn(&c);              /* read character */
        }
#if CLS1_HISTORY_ENABLED
        /* if not an alphanumeric switch to history  */
        prevInHistory = cBufIdx==0 && cBuf[0]==0x1b && cBuf[1]==0x5b && (cBuf[2]==0x41 /*up*/ || cBuf[2]==0x44 /*left*/);
        /* up:    0x27 0x5b 0x41
         * down:  0x27 0x5b 0x42
         * right: 0x27 0x5b 0x43
         * left:  0x27 0x5b 0x44
         */
        if (prevInHistory) {
          UTIL1_strcpy(bufStart, CLS1_HIST_LEN, CLS1_history[CLS1_history_index]);
          CLS1_history_index++;        /* update the index */
          if (CLS1_history_index==CLS1_NOF_HISTORY) {
            CLS1_history_index = 0;
          }
        } else {
          if (CLS1_history_index==0) {
            CLS1_history_index = (CLS1_NOF_HISTORY-1);
          } else {
            CLS1_history_index--;
          }
          UTIL1_strcpy(bufStart, CLS1_HIST_LEN, CLS1_history[CLS1_history_index]);
        }
        bufSize = bufSize + buf - bufStart - UTIL1_strlen(bufStart); /* update the buffer */
        buf = bufStart + UTIL1_strlen(bufStart);
#endif
#if CLS1_ECHO_ENABLED
        CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
        CLS1_PrintPrompt(io);
        CLS1_SendStr(bufStart, io->stdOut);
#endif
      }
    } /* for */
    *buf = '\0';                       /* zero terminate string */
    return TRUE;
  } else {
    return FALSE;
  }
}
Esempio n. 3
0
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 */
  }
}
Esempio n. 4
0
static void TRACE_PrintStatus(const CLS1_StdIOType *io) {
    CLS1_SendStatusStr((unsigned char*)"Trace", (unsigned char*)"\r\n", io->stdOut);
    CLS1_SendStatusStr((unsigned char*)"  channel", (unsigned char*)"", io->stdOut);
    if (traceChannel==TRACE_TO_NONE) {
        CLS1_SendStr((unsigned char*)"NONE\r\n", io->stdOut);
    } else if (traceChannel==TRACE_TO_SHELL) {
        CLS1_SendStr((unsigned char*)"SHELL\r\n", io->stdOut);
    }
    CLS1_SendStatusStr((unsigned char*)"  options", (unsigned char*)"", io->stdOut);
    /*! \todo Extend with motor option */
#if PL_HAS_MOTOR
    if (traceMotor) {
        CLS1_SendStr((unsigned char*)"motor(on) ", io->stdOut);
    } else {
        CLS1_SendStr((unsigned char*)"motor(off) ", io->stdOut);
    }
#endif
#if PL_HAS_MOTOR_TACHO
    if (traceTacho) {
        CLS1_SendStr((unsigned char*)"tacho(on) ", io->stdOut);
    } else {
        CLS1_SendStr((unsigned char*)"tacho(off) ", io->stdOut);
    }
#endif
#if PL_HAS_QUADRATURE
    if (traceQuad) {
        CLS1_SendStr((unsigned char*)"quad(on) ", io->stdOut);
    } else {
        CLS1_SendStr((unsigned char*)"quad(off) ", io->stdOut);
    }
#endif
#if PL_HAS_ACCEL
    if (traceAccel) {
        CLS1_SendStr((unsigned char*)"accel(on) ", io->stdOut);
    } else {
        CLS1_SendStr((unsigned char*)"accel(off) ", io->stdOut);
    }
#endif
    CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
}
Esempio n. 5
0
byte MCP4728_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  const unsigned char *p;

  if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "MCP4728 help")==0) {
    *handled = TRUE;
    return PrintHelp(io);
  } else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "MCP4728 status")==0)) {
    *handled = TRUE;
    return PrintStatus(io);
  } else if (UTIL1_strcmp((char*)cmd, "MCP4728 reset")==0) {
    *handled = TRUE;
    if (MCP4728_Reset()!=ERR_OK) {
      return ERR_FAILED;
    } else {
      return ERR_OK;
    }
  } else if (UTIL1_strcmp((char*)cmd, "MCP4728 wakeup")==0) {
    *handled = TRUE;
    if (MCP4728_Wakeup()!=ERR_OK) {
      return ERR_FAILED;
    } else {
      return ERR_OK;
    }
  } else if (UTIL1_strcmp((char*)cmd, "MCP4728 update")==0) {
    *handled = TRUE;
    if (MCP4728_Update()!=ERR_OK) {
      return ERR_FAILED;
    } else {
      return ERR_OK;
    }
  } else if (UTIL1_strncmp((char*)cmd, "MCP4728 fastwrite all ", sizeof("MCP4728 fastwrite all ")-1)==0) {
    uint16_t dac[4];
    uint8_t pd[4];
    int i;

    *handled = TRUE;
    p = cmd+sizeof("MCP4728 fastwrite all ")-1;
    for(i=0;i<4;i++) { /* init */
      dac[i] = 0;
      pd[i] = 0;
    }
    for(i=0;i<4;i++) {
      if (UTIL1_ScanHex16uNumber(&p, &dac[i])!=ERR_OK) {
        break;
      }
      dac[i] &= MCP4728_MAX_DAC_VAL; /* ensure it is 12 bits */
    }
    if (i!=4) {
      CLS1_SendStr((unsigned char*)"**** Not enough values, 4 expected!\r\n", io->stdErr);
      return ERR_FAILED;
    }
    if (MCP4728_FastWriteAllDAC(dac, sizeof(dac), pd, sizeof(pd))!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"**** FastWrite failed.\r\n", io->stdErr);
      return ERR_FAILED;
    } else {
      return ERR_OK;
    }
  } else if (UTIL1_strncmp((char*)cmd, "MCP4728 fastwrite ", sizeof("MCP4728 fastwrite ")-1)==0) {
    /* e.g. MCP4728 fastwrite 2 0x512 */
    uint16_t dac=0;
    uint8_t channel=0;

    *handled = TRUE;
    p = cmd+sizeof("MCP4728 fastwrite ")-1;
    if (UTIL1_ScanDecimal8uNumber(&p, &channel)!=ERR_OK || channel>3) {
      CLS1_SendStr((unsigned char*)"**** error parsing channel, must be 0, 1, 2 or 3!\r\n", io->stdErr);
      return ERR_FAILED;
    }
    if (UTIL1_ScanHex16uNumber(&p, &dac)!=ERR_OK || dac>MCP4728_MAX_DAC_VAL) {
      CLS1_SendStr((unsigned char*)"**** error parsing 12bit hex value, must start with 0x!\r\n", io->stdErr);
      return ERR_FAILED;
    }
    dac &= MCP4728_MAX_DAC_VAL; /* ensure it is 12 bits */
    if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"**** Write channel DAC failed!\r\n", io->stdErr);
      return ERR_FAILED;
    } else {
      return ERR_OK;
    }
  } else if (UTIL1_strncmp((char*)cmd, "MCP4728 write ", sizeof("MCP4728 write ")-1)==0) {
    uint16_t dac;
    uint8_t ch, res;

    *handled = TRUE;
    res = ERR_OK;
    p = cmd+sizeof("MCP4728 write ")-1;
    if (UTIL1_ScanDecimal8uNumber(&p, &ch)!=ERR_OK || ch>3) {
      CLS1_SendStr((unsigned char*)"**** Failed reading channel. Must be 0, 1, 2 or 3\r\n", io->stdErr);
      res = ERR_FAILED;
    } else {
      if (UTIL1_ScanHex16uNumber(&p, &dac)!=ERR_OK) {
        CLS1_SendStr((unsigned char*)"**** Failed DAC value, must start with 0x\r\n", io->stdErr);
        res = ERR_FAILED;
      } else {
        if (MCP4728_WriteDACandEE(ch, dac)!=ERR_OK) {
          CLS1_SendStr((unsigned char*)"**** Write failed.\r\n", io->stdErr);
          res = ERR_FAILED;
        }
      }
    }
    return res;
  }
  return ERR_OK;
}
Esempio n. 6
0
byte HOVAL_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "Hoval help")==0) {
    CLS1_SendHelpStr((unsigned char*)"Hoval", (const unsigned char*)"Group of Hoval commands\r\n", io->stdOut);
    CLS1_SendHelpStr((unsigned char*)"  help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
    CLS1_SendHelpStr((unsigned char*)"  scheduler (on|off)", (const unsigned char*)"Turns time scheduler on or off\r\n", io->stdOut);
    CLS1_SendHelpStr((unsigned char*)"  time <start> <end>", (const unsigned char*)"Sets the 'on' time, e.g. '21:00:00 21:30:00'\r\n", io->stdOut);
#if PL_HAS_DATE_SUPPORT
    CLS1_SendHelpStr((unsigned char*)"  date (on|off) <date>", (const unsigned char*)"Sets the on and off date\r\n", io->stdOut);
#endif
#if 0
    CLS1_SendHelpStr((unsigned char*)"  relay (1|2) (on|off)", (const unsigned char*)"Turn relay on or off\r\n", io->stdOut);
#endif
    CLS1_SendHelpStr((unsigned char*)"  pump (on|off)", (const unsigned char*)"Turn pump on or off\r\n", io->stdOut);
    CLS1_SendHelpStr((unsigned char*)"  cooling (on|off)", (const unsigned char*)"Turns cooling on or off\r\n", io->stdOut);
    *handled = TRUE;
    return ERR_OK;
  } else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "Hoval status")==0)) {
    *handled = TRUE;
    return PrintStatus(io);
  } else if (UTIL1_strcmp((char*)cmd, "Hoval relay 1 on")==0) {
    DisableScheduler(io);
    Button_On(TRUE, io);
    *handled = TRUE;
    return ERR_OK;
#if 0
  } else if (UTIL1_strcmp((char*)cmd, "Hoval relay 1 off")==0) {
    DisableScheduler(io);
    Button_On(FALSE, io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval relay 2 on")==0) {
    DisableScheduler(io);
    Pump_On(TRUE, io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval relay 2 off")==0) {
    DisableScheduler(io);
    Button_On(FALSE, io);
    *handled = TRUE;
    return ERR_OK;
#endif
  } else if (UTIL1_strcmp((char*)cmd, "Hoval pump on")==0) {
    DisableScheduler(io);
    SetCooling(TRUE, io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval pump off")==0) {
    DisableScheduler(io);
    SetCooling(FALSE, io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval scheduler on")==0) {
    mySchedule.isSchedulerOn = TRUE;
    if (PutRTCData(&mySchedule)!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"*** Failure setting schedule data\r\n", io->stdErr);
      return ERR_FAILED;
    }
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval scheduler off")==0) {
    mySchedule.isSchedulerOn = FALSE;
    if (PutRTCData(&mySchedule)!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"*** Failure setting schedule data\r\n", io->stdErr);
      return ERR_FAILED;
    }
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval cooling on")==0) {
    DisableScheduler(io);
    SetCooling(TRUE, io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strcmp((char*)cmd, "Hoval cooling off")==0) {
    SetCooling(FALSE, io);
    DisableScheduler(io);
    *handled = TRUE;
    return ERR_OK;
  } else if (UTIL1_strncmp((char*)cmd, "Hoval time", sizeof("Hoval time")-1)==0) {
    *handled = TRUE;
    return TimeOnCmd(cmd+sizeof("Hoval time")-1, io);
#if PL_HAS_DATE_SUPPORT
  } else if (UTIL1_strncmp((char*)cmd, "Hoval date", sizeof("Hoval date")-1)==0) {
    *handled = TRUE;
    return DateCmd(cmd+sizeof("Hoval date")-1, io);
#endif
  }
  return ERR_OK;
}
Esempio n. 7
0
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*)"  state", REF_GetStateString(), io->stdOut);
  CLS1_SendStr((unsigned char*)"\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);

  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);

  CLS1_SendStatusStr((unsigned char*)"  raw val", (unsigned char*)"", io->stdOut);
  for (i=0;i<REF_NOF_SENSORS;i++) {
    if (i==0) {
      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);
  for (i=0;i<REF_NOF_SENSORS;i++) {
    if (i==0) {
      CLS1_SendStr((unsigned char*)"0x", io->stdOut);
    } else {
      CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
    }
    buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.minVal[i]);
    CLS1_SendStr(buf, io->stdOut);
  }
  CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
  CLS1_SendStatusStr((unsigned char*)"  max val", (unsigned char*)"", io->stdOut);
  for (i=0;i<REF_NOF_SENSORS;i++) {
    if (i==0) {
      CLS1_SendStr((unsigned char*)"0x", io->stdOut);
    } else {
      CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
    }
    buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMax.maxVal[i]);
    CLS1_SendStr(buf, io->stdOut);
  }
  CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
 
  CLS1_SendStatusStr((unsigned char*)"  calib val", (unsigned char*)"", io->stdOut);
  for (i=0;i<REF_NOF_SENSORS;i++) {
    if (i==0) {
      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);
  return ERR_OK;
}
Esempio n. 8
0
static void Serial_println(const char *msg) {
  CLS1_ConstStdIOType *io = CLS1_GetStdio();

  CLS1_SendStr((const unsigned char*)msg, io->stdOut);
  CLS1_SendStr((const unsigned char*)"\r\n", io->stdOut);
}
Esempio n. 9
0
static void RfidTask(void *pvParameters) {
  MFRC522_StatusCode status;
  uint8_t last_uidBytes[MFRC522_UID_MAX_NOF_BYTES];
  int i;
  bool newCardDetected = FALSE;

  (void)pvParameters; /* not used */
  init_uid_bytes(last_uidBytes);
  RFID_uid = NULL;

  vTaskDelay(pdMS_TO_TICKS(1000)); /* give hardware some time to power-up */
  MFRC522_PCD_Init();
  //MFRC522_SetAntennaGain(MFRC522_RxGain_max);
  for(;;) {
    newCardDetected = FALSE;
    RFID_GetLock();
    if (RFID_DoSelfTest) {
      if (!MFRC522_PCD_PerformSelfTest()) {
        CLS1_SendStr("Selftest failed()!\r\n", CLS1_GetStdio()->stdErr);
      }
    }
    if (MFRC522_PICC_IsNewCardPresent()) { /* check if a card responds to PICC_CMD_REQA */
      //CLS1_SendStr("Card detected...\r\n", CLS1_GetStdio()->stdOut);
      if (MFRC522_PICC_ReadCardSerial()) {
        //CLS1_SendStr("Read card serial...\r\n", CLS1_GetStdio()->stdOut);
        RFID_uid = MFRC522_GetUid();

        newCardDetected = TRUE;
        if (RFID_dumpNewCardInformation) {
          MFRC522_PICC_DumpToSerial(RFID_uid);
        } else if (RFID_changeUID) { /* only works with Chinese cards which have the UID writeable! */
          if (!MFRC522_MIFARE_SetUid(RFID_newUID4, sizeof(RFID_newUID4), TRUE)) {
            CLS1_SendStr("Setting new uid failed!\r\n", CLS1_GetStdio()->stdOut);
          } else {
            CLS1_SendStr("Set New ID to card!\r\n", CLS1_GetStdio()->stdOut);
          }
        } else if (RFID_authUID) { /* only for NTAG216! */
          AuthNTag216(RFID_uid);
        } else if (RFID_WriteUltraLight) {
          WriteUltraLight(RFID_uid);
        } else if (RFID_WriteMifare) {
          WriteMifare(RFID_uid);
        } else if (RFID_TryKnownKeys) {
          TryKnownKeys();
        } else if (!same_uid_bytes(last_uidBytes, &RFID_uid->uidByte[0], RFID_uid->size)) {
          CLS1_SendStr("\r\nCard UID:", CLS1_GetStdio()->stdOut);
          dump_byte_array(RFID_uid->uidByte, RFID_uid->size, TRUE);
          CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
          init_uid_bytes(last_uidBytes);

          set_uid_bytes(last_uidBytes, &RFID_uid->uidByte[0], RFID_uid->size);

          CLS1_SendStr("PICC type: ", CLS1_GetStdio()->stdOut);
          PICC_Type piccType = MFRC522_PICC_GetType(RFID_uid->sak);
          CLS1_SendStr(MFRC522_PICC_GetTypeName(piccType), CLS1_GetStdio()->stdOut);
          CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
        } else { /* same card again */
          CLS1_SendStr(".", CLS1_GetStdio()->stdOut);
        }
      }
      MFRC522_PICC_HaltA(); /* Halt PICC */
      MFRC522_PCD_StopCrypto1(); /* Stop encryption on PCD */
    } /* new card present */
    RFID_ReleaseLock();
    if (!newCardDetected) {
      vTaskDelay(pdMS_TO_TICKS(500));
    } else {
      vTaskDelay(pdMS_TO_TICKS(2000));
    }
  }
}
Esempio n. 10
0
/*
 * Try using the PICC (the tag/card) with the given key to access block 0.
 * On success, it will show the key details, and dump the block data on Serial.
 *
 * @return true when the given key worked, false otherwise.
 */
bool try_key(MFRC522_MIFARE_Key *key) {
    bool result = false;
    uint8_t buffer[18];
    uint8_t block = 0;
    MFRC522_StatusCode status;

    // http://arduino.stackexchange.com/a/14316
  //  if (!MFRC522_PICC_IsNewCardPresent()) {
  //      return FALSE;
  //  }
  //  if (!MFRC522_PICC_ReadCardSerial()) {
  //      return FALSE;
  //  }
    CLS1_SendStr("Authenticating using key A...\r\n", CLS1_GetStdio()->stdOut);
    status = MFRC522_PCD_Authenticate(MFRC522_PICC_CMD_MF_AUTH_KEY_A, block, key, MFRC522_GetUid());
    if (status != STATUS_OK) {
      CLS1_SendStr("PCD_Authenticate() failed: ", CLS1_GetStdio()->stdErr);
      CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdErr);
      CLS1_SendStr("\r\n", CLS1_GetStdio()->stdErr);
      return false;
    }
    // Read block
    uint8_t byteCount = sizeof(buffer);
    status = MFRC522_MIFARE_Read(block, buffer, &byteCount);
    if (status != STATUS_OK) {
      CLS1_SendStr("MIFARE_Read() failed: ", CLS1_GetStdio()->stdErr);
      CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdErr);
    } else {
        // Successful read
        result = true;
        CLS1_SendStr("Success with key:", CLS1_GetStdio()->stdOut);
        dump_byte_array((*key).keyByte, MF_KEY_SIZE, TRUE);
        CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
        // Dump block data
        CLS1_SendStr("Block ", CLS1_GetStdio()->stdOut);
        CLS1_SendNum8u(block, CLS1_GetStdio()->stdOut);
        CLS1_SendStr(":", CLS1_GetStdio()->stdOut);
        dump_byte_array(buffer, 16, TRUE);
        CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
    }
    CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
    MFRC522_PICC_HaltA();       // Halt PICC
    MFRC522_PCD_StopCrypto1();  // Stop encryption on PCD
    return result;
}
Esempio n. 11
0
static void WriteMifare(Uid *uid) {
  MFRC522_MIFARE_Key key;

  PICC_Type piccType = MFRC522_PICC_GetType(uid->sak);
  if (   piccType != PICC_TYPE_MIFARE_MINI
      && piccType != PICC_TYPE_MIFARE_1K
      && piccType != PICC_TYPE_MIFARE_UL
      )
  {
    CLS1_SendStr("Only works with MIFARE classic cards!\r\n", CLS1_GetStdio()->stdErr);
    return;
  }
  // init default access key
  for(int i=0;i<sizeof(key.keyByte);i++) {
    key.keyByte[i] = 0xff;
  }

  // In this sample we use the second sector,
  // that is: sector #1, covering block #4 up to and including block #7
  uint8_t sector         = 1;
  uint8_t blockAddr      = 4;
  uint8_t dataBlock[]    = {
      0x01, 0x02, 0x03, 0x04, //  1,  2,   3,  4,
      0x05, 0x06, 0x07, 0x08, //  5,  6,   7,  8,
      0x09, 0x0a, 0xff, 0x0b, //  9, 10, 255, 11,
      0x0c, 0x0d, 0x0e, 0x0f  // 12, 13, 14, 15
  };
  uint8_t trailerBlock   = 7;
  MFRC522_StatusCode status;
  uint8_t buffer[18];
  uint8_t size = sizeof(buffer);

  // Authenticate using key A
  CLS1_SendStr("Authenticating using key A...\r\n", CLS1_GetStdio()->stdOut);
  status = (MFRC522_StatusCode) MFRC522_PCD_Authenticate(MFRC522_PICC_CMD_MF_AUTH_KEY_A, trailerBlock, &key, uid);
  if (status != STATUS_OK) {
      CLS1_SendStr("PCD_Authenticate() failed: ", CLS1_GetStdio()->stdOut);
      CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdOut);
      return;
  }

  // Show the whole sector as it currently is
  CLS1_SendStr("Current data in sector:\r\n", CLS1_GetStdio()->stdOut);
  MFRC522_PICC_DumpMifareClassicSectorToSerial(uid, &key, sector);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);

  // Read data from the block
  CLS1_SendStr("Reading data from block ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(blockAddr, CLS1_GetStdio()->stdOut);
  CLS1_SendStr(" ...\r\n", CLS1_GetStdio()->stdOut);
  status = (MFRC522_StatusCode) MFRC522_MIFARE_Read(blockAddr, buffer, &size);
  if (status != STATUS_OK) {
    CLS1_SendStr("MIFARE_Read() failed: ", CLS1_GetStdio()->stdOut);
    CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdOut);
    CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  }
  CLS1_SendStr("Data in block ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(blockAddr, CLS1_GetStdio()->stdOut);
  CLS1_SendStr(":\r\n", CLS1_GetStdio()->stdOut);
  dump_byte_array(buffer, 16, TRUE);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);

  // Authenticate using key B
  CLS1_SendStr("Authenticating again using key B...\r\n", CLS1_GetStdio()->stdOut);
  status = (MFRC522_StatusCode) MFRC522_PCD_Authenticate(MFRC522_PICC_CMD_MF_AUTH_KEY_B, trailerBlock, &key, uid);
  if (status != STATUS_OK) {
      CLS1_SendStr("PCD_Authenticate() failed: ", CLS1_GetStdio()->stdOut);
      CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdOut);
      CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
      return;
  }

  // Write data to the block
  CLS1_SendStr("Writing data into block ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(blockAddr, CLS1_GetStdio()->stdOut);
  CLS1_SendStr(" ...\r\n", CLS1_GetStdio()->stdOut);
  dump_byte_array(dataBlock, 16, TRUE);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  status = (MFRC522_StatusCode) MFRC522_MIFARE_Write(blockAddr, dataBlock, 16);
  if (status != STATUS_OK) {
    CLS1_SendStr("MIFARE_Write() failed: ", CLS1_GetStdio()->stdOut);
    CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdOut);
    CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  }
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);

  // Read data from the block (again, should now be what we have written)
  CLS1_SendStr("Reading data from block ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(blockAddr, CLS1_GetStdio()->stdOut);
  CLS1_SendStr(" ...\r\n", CLS1_GetStdio()->stdOut);
  status = (MFRC522_StatusCode) MFRC522_MIFARE_Read(blockAddr, buffer, &size);
  if (status != STATUS_OK) {
    CLS1_SendStr("MIFARE_Read() failed: ", CLS1_GetStdio()->stdOut);
    CLS1_SendStr(MFRC522_GetStatusCodeName(status), CLS1_GetStdio()->stdOut);
    CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  }
  CLS1_SendStr("Data in block ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(blockAddr, CLS1_GetStdio()->stdOut);
  CLS1_SendStr(":\r\n", CLS1_GetStdio()->stdOut);
  dump_byte_array(buffer, 16, TRUE);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);

  // Check that data in block is what we have written
  // by counting the number of bytes that are equal
  CLS1_SendStr("Checking result...", CLS1_GetStdio()->stdOut);
  uint8_t count = 0;
  for (uint8_t i = 0; i < 16; i++) {
      // Compare buffer (= what we've read) with dataBlock (= what we've written)
      if (buffer[i] == dataBlock[i]) {
          count++;
      }
  }
  CLS1_SendStr("Number of bytes that match = ", CLS1_GetStdio()->stdOut);
  CLS1_SendNum8u(count, CLS1_GetStdio()->stdOut);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
  if (count == 16) {
    CLS1_SendStr("Success :-)\r\n", CLS1_GetStdio()->stdOut);
  } else {
    CLS1_SendStr("Failure, no match :-(\r\n", CLS1_GetStdio()->stdOut);
    CLS1_SendStr("  perhaps the write didn't work properly...\r\n", CLS1_GetStdio()->stdOut);
  }
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);

  // Dump the sector data
  CLS1_SendStr("Current data in sector:\r\n", CLS1_GetStdio()->stdOut);
  MFRC522_PICC_DumpMifareClassicSectorToSerial(uid, &key, sector);
  CLS1_SendStr("\r\n", CLS1_GetStdio()->stdOut);
}
Esempio n. 12
0
void RAPP_SniffPacket(RPHY_PacketDesc *packet, bool isTx) {
#if PL_HAS_SHELL
  uint8_t buf[32];
  const CLS1_StdIOType *io;
  int i;
  uint8_t dataSize;
  RNWK_ShortAddrType addr;
  
  io = CLS1_GetStdio();
  if (isTx) {
    CLS1_SendStr((unsigned char*)"Packet Tx ", io->stdOut);
  } else {
    CLS1_SendStr((unsigned char*)"Packet Rx ", io->stdOut);
  }
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"flags: ");
  UTIL1_strcatNum16s(buf, sizeof(buf), packet->flags);
  CLS1_SendStr(buf, io->stdOut);
  if (packet->flags!=RPHY_PACKET_FLAGS_NONE) {
    CLS1_SendStr((unsigned char*)"(", io->stdOut);
    if (packet->flags&RPHY_PACKET_FLAGS_IS_ACK) {
      CLS1_SendStr((unsigned char*)"IS_ACK,", io->stdOut);
    }
    if (packet->flags&RPHY_PACKET_FLAGS_REQ_ACK) {
      CLS1_SendStr((unsigned char*)"REQ_ACK", io->stdOut);
    }
    CLS1_SendStr((unsigned char*)")", io->stdOut);
  }
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" size: ");
  UTIL1_strcatNum16s(buf, sizeof(buf), packet->phySize);
  CLS1_SendStr(buf, io->stdOut);
  /* PHY */
  CLS1_SendStr((unsigned char*)" PHY data: ", io->stdOut);
  dataSize = RPHY_BUF_SIZE(packet->phyData);
  for(i=0; i<dataSize+RPHY_HEADER_SIZE;i++) {
    buf[0] = '\0';
    UTIL1_strcatNum8Hex(buf, sizeof(buf), packet->phyData[i]);
    UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ");
    CLS1_SendStr(buf, io->stdOut);
  }
  /* MAC */
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" MAC size:");
  UTIL1_strcatNum8u(buf, sizeof(buf), dataSize);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" type:");
  UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_TYPE(packet->phyData));
  CLS1_SendStr(buf, io->stdOut);
  RMAC_DecodeType(buf, sizeof(buf), packet);
  CLS1_SendStr(buf, io->stdOut);
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" s#:");
  UTIL1_strcatNum8Hex(buf, sizeof(buf), RMAC_BUF_SEQN(packet->phyData));
  CLS1_SendStr(buf, io->stdOut);
  /* NWK */
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" NWK src:");
  addr = RNWK_BUF_GET_SRC_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
  UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
  UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" dst:");
  addr = RNWK_BUF_GET_DST_ADDR(packet->phyData);
#if RNWK_SHORT_ADDR_SIZE==1
  UTIL1_strcatNum8Hex(buf, sizeof(buf), addr);
#else
  UTIL1_strcatNum16Hex(buf, sizeof(buf), addr);
#endif
  CLS1_SendStr(buf, io->stdOut);
  /* APP */
  if (dataSize>RMAC_HEADER_SIZE+RNWK_HEADER_SIZE) { /* there is application data */
    UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)" APP type:");
    UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_TYPE(packet->phyData));
    UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" size:");
    UTIL1_strcatNum8Hex(buf, sizeof(buf), RAPP_BUF_SIZE(packet->phyData));
    CLS1_SendStr(buf, io->stdOut);
  }
  CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#else
  (void)packet;
  (void)isTx;
#endif
}
Esempio n. 13
0
static void CheckButton(void) {
#if PL_HAS_USER_BUTTON
  uint32_t timeTicks; /* time in ticks */
  #define BUTTON_CNT_MS  100  /* iteration count for button */
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
  bool autoCalibrate = FALSE;
#endif
  
  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 */
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
      autoCalibrate = FALSE;
#endif
      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 */
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
      } 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;
#endif
#if PL_APP_LINE_MAZE
      } else if (timeTicks>=(3000/BUTTON_CNT_MS)) {
        CLS1_SendStr((unsigned char*)"delete solution.\r\n", CLS1_GetStdio()->stdOut);
        MAZE_ClearSolution();
#endif
      }
      while (SW1_GetVal()==0) { /* wait until button is released */
        FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
      }
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
      if (autoCalibrate) {
        /* perform automatic calibration */
        WAIT1_WaitOSms(1500); /* wait some time so the user can remove the hand from the button */
        AutoCalibrateReflectance(CLS1_GetStdio());
      }
#endif
    }
  } /* if */
#endif
}
Esempio n. 14
0
uint8_t RNETA_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
  uint8_t res = ERR_OK;
  const uint8_t *p;
  uint16_t val16;
  uint8_t val8;

  if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"app help")==0) {
    PrintHelp(io);
    *handled = TRUE;
  } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"app status")==0) {
    *handled = TRUE;
    return PrintStatus(io);
  } else if (UTIL1_strncmp((char*)cmd, (char*)"app saddr", sizeof("app saddr")-1)==0) {
    p = cmd + sizeof("app saddr")-1;
    *handled = TRUE;
    if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
      (void)RNWK_SetThisNodeAddr((RNWK_ShortAddrType)val16);
    } else {
      CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
      return ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"app send val", sizeof("app send val")-1)==0) {
    p = cmd + sizeof("app send val")-1;
    *handled = TRUE;
    if (UTIL1_ScanDecimal8uNumber(&p, &val8)==ERR_OK) {
      (void)RAPP_SendPayloadDataBlock(&val8, sizeof(val8), RAPP_MSG_TYPE_DATA, APP_dstAddr, RPHY_PACKET_FLAGS_NONE); /* only send low byte */
    } else {
      CLS1_SendStr((unsigned char*)"ERR: wrong number format\r\n", io->stdErr);
      return ERR_FAILED;
    }
  } else if (UTIL1_strncmp((char*)cmd, (char*)"app daddr", sizeof("app daddr")-1)==0) {
    p = cmd + sizeof("app daddr")-1;
    *handled = TRUE;
    if (UTIL1_ScanHex16uNumber(&p, &val16)==ERR_OK) {
      APP_dstAddr = val16;
    } else {
      CLS1_SendStr((unsigned char*)"ERR: wrong address\r\n", io->stdErr);
      return ERR_FAILED;
    }
#if RNET_CONFIG_REMOTE_STDIO
  } else if (UTIL1_strncmp((char*)cmd, (char*)"app send", sizeof("app send")-1)==0) {
    unsigned char buf[32];
    RSTDIO_QueueType queue;
    
    if (UTIL1_strncmp((char*)cmd, (char*)"app send in", sizeof("app send in")-1)==0) {
      queue = RSTDIO_QUEUE_TX_IN;
      cmd += sizeof("app send in");
    } else if (UTIL1_strncmp((char*)cmd, (char*)"app send out", sizeof("app send out")-1)==0) {
      queue = RSTDIO_QUEUE_TX_OUT;      
      cmd += sizeof("app send out");
    } else if (UTIL1_strncmp((char*)cmd, (char*)"app send err", sizeof("app send err")-1)==0) {
      queue = RSTDIO_QUEUE_TX_ERR;      
      cmd += sizeof("app send err");
    } else {
      return ERR_OK; /* not handled */
    }
    UTIL1_strcpy(buf, sizeof(buf), cmd);
    UTIL1_chcat(buf, sizeof(buf), '\n');
    buf[sizeof(buf)-2] = '\n'; /* have a '\n' in any case */
    if (RSTDIO_SendToTxStdio(queue, buf, UTIL1_strlen((char*)buf))!=ERR_OK) {
      CLS1_SendStr((unsigned char*)"failed!\r\n", io->stdErr);
    }
    *handled = TRUE;
#endif
  }
  return res;
}
Esempio n. 15
0
File: Drive.c Progetto: 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;
}
Esempio n. 16
0
/*! \brief Simple benchmark function: first we are going to write a file, then we will copy it */
static void benchmark(const CLS1_StdIOType *io) {
  static FIL fp;
  uint16_t i;
  UINT bw;
  uint8_t read_buf[10];
  TIMEREC time, startTime;
  int32_t start_mseconds, mseconds;

  /* write benchmark */
  CLS1_SendStr((const unsigned char*)"Benchmark: open file, write 10k times 10 bytes (100'000 bytes), close file:\r\n", io->stdOut);
  CLS1_SendStr((const unsigned char*)"Deleting any existing files...\r\n", io->stdOut);
  (void)FAT1_DeleteFile((const unsigned char*)"./bench.txt", io);
  (void)FAT1_DeleteFile((const unsigned char*)"./copy.txt", io);

  CLS1_SendStr((const unsigned char*)"Creating benchmark file...\r\n", io->stdOut);
  (void)TmDt1_GetTime(&startTime);
  if (FAT1_open(&fp, "./bench.txt", FA_CREATE_ALWAYS|FA_WRITE)!=FR_OK) {
    CLS1_SendStr((const unsigned char*)"*** Failed opening benchmark file!\r\n", io->stdErr);
    return;
  }
  for(i=0;i<10000;i++) {
    if (FAT1_write(&fp, "benchmark ", sizeof("benchmark ")-1, &bw)!=FR_OK) {
      CLS1_SendStr((const unsigned char*)"*** Failed writing file!\r\n", io->stdErr);
      (void)FAT1_close(&fp);
      return;
    }
  }
  (void)FAT1_close(&fp);
  (void)TmDt1_GetTime(&time);
  start_mseconds = startTime.Hour*60*60*1000 + startTime.Min*60*1000 + startTime.Sec*1000 + startTime.Sec100*10;
  mseconds = time.Hour*60*60*1000 + time.Min*60*1000 + time.Sec*1000 + time.Sec100*10 - start_mseconds;
  CLS1_SendNum32s(mseconds, io->stdOut);
  CLS1_SendStr((const unsigned char*)" mseconds needed for command.\r\n", io->stdOut);

  /* read benchmark */
  CLS1_SendStr((const unsigned char*)"Reading benchmark file...\r\n", io->stdOut);
  (void)TmDt1_GetTime(&startTime);
  if (FAT1_open(&fp, "./bench.txt", FA_READ)!=FR_OK) {
    CLS1_SendStr((const unsigned char*)"*** Failed opening benchmark file!\r\n", io->stdErr);
    return;
  }
  for(i=0;i<10000;i++) {
    if (FAT1_read(&fp, &read_buf[0], sizeof(read_buf), &bw)!=FR_OK) {
      CLS1_SendStr((const unsigned char*)"*** Failed reading file!\r\n", io->stdErr);
      (void)FAT1_close(&fp);
      return;
    }
  }
  (void)FAT1_close(&fp);
  (void)TmDt1_GetTime(&time);
  start_mseconds = startTime.Hour*60*60*1000 + startTime.Min*60*1000 + startTime.Sec*1000 + startTime.Sec100*10;
  mseconds = time.Hour*60*60*1000 + time.Min*60*1000 + time.Sec*1000 + time.Sec100*10 - start_mseconds;
  CLS1_SendNum32s(mseconds, io->stdOut);
  CLS1_SendStr((const unsigned char*)" mseconds needed for command.\r\n", io->stdOut);

  /* copy benchmark */
  CLS1_SendStr((const unsigned char*)"Benchmark: copy file (100'000 bytes):\r\n", io->stdOut);
  CLS1_SendStr((const unsigned char*)"Going to copy file...\r\n", io->stdOut);
  (void)TmDt1_GetTime(&startTime);
  (void)FAT1_CopyFile((const unsigned char*)"./bench.txt", (const unsigned char*)"./copy.txt", io);
  (void)TmDt1_GetTime(&time);
  start_mseconds = startTime.Hour*60*60*1000 + startTime.Min*60*1000 + startTime.Sec*1000 + startTime.Sec100*10;
  mseconds = time.Hour*60*60*1000 + time.Min*60*1000 + time.Sec*1000 + time.Sec100*10 - start_mseconds;
  CLS1_SendNum32s(mseconds, io->stdOut);
  CLS1_SendStr((const unsigned char*)" mseconds needed for command.\r\n", io->stdOut);
  CLS1_SendStr((const unsigned char*)"done!\r\n", io->stdOut);
}
Esempio n. 17
0
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
    uint8_t res = ERR_OK;
    long val;
    const unsigned char *p;

    if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor help")==0) {
        MOT_PrintHelp(io);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor status")==0) {
        MOT_PrintStatus(io);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L forward")==0) {
        MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R forward")==0) {
        MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L backward")==0) {
        MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R backward")==0) {
        MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
        *handled = TRUE;
    } else if (UTIL1_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
        if (!isMotorOn) {
            CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
            res = ERR_FAILED;
        } else {
            p = cmd+sizeof("motor L duty");
            if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
                MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
                *handled = TRUE;
            } else {
                CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
                res = ERR_FAILED;
            }
        }
    } else if (UTIL1_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
        if (!isMotorOn) {
            CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
            res = ERR_FAILED;
        } else {
            p = cmd+sizeof("motor R duty");
            if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
                MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
                *handled = TRUE;
            } else {
                CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
                res = ERR_FAILED;
            }
        }
    } else if (UTIL1_strncmp((char*)cmd, (char*)"motor on", sizeof("motor on")-1)==0) {
        isMotorOn = TRUE;
        *handled = TRUE;
    } else if (UTIL1_strncmp((char*)cmd, (char*)"motor off", sizeof("motor off")-1)==0) {
        MOT_SetSpeedPercent(&motorL, 0);
        MOT_SetSpeedPercent(&motorR, 0);
        isMotorOn = FALSE;
        *handled = TRUE;
#if PL_HAS_MOTOR_BRAKE
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake on")==0) {
        MOT_SetBrake(&motorL, TRUE);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake off")==0) {
        MOT_SetBrake(&motorL, FALSE);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake on")==0) {
        MOT_SetBrake(&motorR, TRUE);
        *handled = TRUE;
    } else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake off")==0) {
        MOT_SetBrake(&motorR, FALSE);
        *handled = TRUE;
#endif
    }
    return res;
}
Esempio n. 18
0
void Serial_print(const char *msg) {
  CLS1_SendStr((const unsigned char*)msg, CLS1_GetStdio()->stdOut);
}
Esempio n. 19
0
static portTASK_FUNCTION(ShellTask, pvParameters) {
#if PL_HAS_USB_CDC
  static unsigned char cdc_buf[48];
#endif
#if PL_HAS_BLUETOOTH
  static unsigned char bluetooth_buf[48];
#endif
  static unsigned char localConsole_buf[48];
#if CLS1_DEFAULT_SERIAL
  CLS1_ConstStdIOTypePtr ioLocal = CLS1_GetStdio();  
#endif
  
  (void)pvParameters; /* not used */
#if PL_HAS_USB_CDC
  cdc_buf[0] = '\0';
#endif
#if PL_HAS_BLUETOOTH
  bluetooth_buf[0] = '\0';
#endif
  localConsole_buf[0] = '\0';
#if CLS1_DEFAULT_SERIAL
  (void)CLS1_ParseWithCommandTable((unsigned char*)CLS1_CMD_HELP, ioLocal, CmdParserTable);
#endif
  for(;;) {
#if CLS1_DEFAULT_SERIAL
    (void)CLS1_ReadAndParseWithCommandTable(localConsole_buf, sizeof(localConsole_buf), ioLocal, CmdParserTable);
#endif
#if PL_HAS_USB_CDC
    (void)CLS1_ReadAndParseWithCommandTable(cdc_buf, sizeof(cdc_buf), &CDC_stdio, CmdParserTable);
#endif
#if PL_HAS_BLUETOOTH
    (void)CLS1_ReadAndParseWithCommandTable(bluetooth_buf, sizeof(bluetooth_buf), &BT_stdio, CmdParserTable);
#endif
#if PL_HAS_SHELL_QUEUE
    {
      /*! \todo Handle shell queue */
  #if PL_SQUEUE_SINGLE_CHAR
      unsigned char ch;

      while((ch=SQUEUE_ReceiveChar()) && ch!='\0') {
        ioLocal->stdOut(ch);
  #if PL_HAS_BLUETOOTH
        BT_stdio.stdOut(ch); /* copy on Bluetooth */
  #endif
  #if PL_HAS_USB_CDC
        CDC_stdio.stdOut(ch); /* copy on USB CDC */
  #endif
      }
  #else
      {
        const unsigned char *msg;

        msg = SQUEUE_ReceiveMessage();
        if (msg!=NULL) {
          CLS1_SendStr(msg, CLS1_GetStdio()->stdOut);
          FRTOS1_vPortFree((void*)msg);
        }
      }
  #endif
    }
#endif
    FRTOS1_vTaskDelay(10/portTICK_RATE_MS);
  } /* for */
}
Esempio n. 20
0
static portTASK_FUNCTION(Hoval, pvParameters) {
  /* LED status:
   * Green: 1 Hz Heartbeat
   * Red: On: pump is on
   *      1 Hz: Scheduler enabled
   *      Off: no scheduler
   */
  CLS1_ConstStdIOType *io = CLS1_GetStdio();
  bool isEnabled = SW1_GetVal()==0; /* initial SW1 status */
  uint16_t secondsOn; /* number of seconds turned on */
  
  (void)pvParameters; /* parameter not used */
  if (GetRTCData(&mySchedule)!=ERR_OK) {
    CLS1_SendStr((unsigned char*)"*** Failed getting RTC data\r\n", io->stdErr);
    for(;;); /* cannot operate like this! */
  }
  isPumpOn = FALSE;
  for(;;) {
    /* check mode switch: turns scheduler on/off */
    if (!isEnabled && SW1_GetVal()==0) {
      Log((unsigned char*)"Switch changed to ON position", io);
    } else if (isEnabled && SW1_GetVal()!=0) {
      Log((unsigned char*)"Switch changed to OFF position", io);
    }
    isEnabled = SW1_GetVal()==0; /* Switch in ON position */

    /* check schedule */
    if (isEnabled && mySchedule.isSchedulerOn) {
      if (!isPumpOn) { /* pump is off, check if we need to turn it on */
        if (isInRange(&mySchedule.on, &mySchedule.off)) {
          SetCooling(TRUE, io);
        }
      } else { /* pump is on, check if we have to turn it off */
        if (!isInRange(&mySchedule.on, &mySchedule.off)) {
          SetCooling(FALSE, io);
        }
      }
    }
    if (isPumpOn) { /* check if we need to toggle the button on the Hoval to avoid timeout */
      secondsOn++;
      if (secondsOn>HOVAL_CYCLE_TIME_SECONDS) {
        Log((unsigned char*)"Toggle manual button to avoid timeout...", io);
        SetCooling(FALSE, io);
        FRTOS1_vTaskDelay(5000/portTICK_RATE_MS); /* wait some time until re-enable manual mode again */
        SetCooling(TRUE, io);
        secondsOn = 0; /* restart counter */
      }
    }
    /* show status LED */
    SD_GreenLed_Neg(); /* 1 Hz heartbeat LED */
    if (isPumpOn) { /* steady red LED if pump is on for cooling */
      SD_RedLed_On();
      LEDR_On();
    } else if (isEnabled && mySchedule.isSchedulerOn) { /* blinking red LED for enabled scheduler */
      SD_RedLed_Put(SD_GreenLed_Get()); /* blink red led in sync with green one */
      LEDR_Put(SD_GreenLed_Get());
    } else {
      SD_RedLed_Off(); /* no red LED if not cooling and no schedule */
      LEDR_Off();
    }
    FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
  }
}
Esempio n. 21
0
void SHELL_SendString(unsigned char *msg) {
  CLS1_SendStr(msg, CLS1_GetStdio()->stdOut);
}
Esempio n. 22
0
void APP_DebugPrint(unsigned char *str) {
  CLS1_SendStr(str, CLS1_GetStdio()->stdOut);
}
Esempio n. 23
0
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
  int32_t pid, speed, speedL, speedR;
#if PID_DEBUG
  unsigned char buf[16];
  static uint8_t cnt = 0;
#endif
  uint8_t errorPercent;
  MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;

  pid = PID(currLine, setLine, config);
  errorPercent = errorWithinPercent(currLine-setLine);

  /* transform into different speed for motors. The PID is used as difference value to the motor PWM */
  if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = speed;
      speedL = speed-pid;
    } else { /* turn left */
      speedR = speed+pid;
      speedL = speed;
    }
  } else if (errorPercent <= 40) {
    /* outside left/right halve position from center, slow down one motor and speed up the other */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = speed+pid; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = speed-pid; /* decrease speed */
    }
  } else if (errorPercent <= 70) {
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
    pid = Limit(pid, -speed, speed);
    if (pid<0) { /* turn right */
      speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
    }
  } else  {
    /* line is far to the left or right: use backward motor motion */
    speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
    if (pid<0) { /* turn right */
      speedR = -speed+pid; /* decrease speed */
      speedL = speed-pid; /* increase speed */
    } else { /* turn left */
      speedR = speed+pid; /* increase speed */
      speedL = -speed-pid; /* decrease speed */
    }
    speedL = Limit(speedL, -speed, speed);
    speedR = Limit(speedR, -speed, speed);
    directionL = AbsSpeed(&speedL);
    directionR = AbsSpeed(&speedR);
  }
  /* speed is now always positive, make sure it is within 16bit PWM boundary */
  if (speedL>0xFFFF) {
    speedL = 0xFFFF;
  } else if (speedL<0) {
    speedL = 0;
  }
  if (speedR>0xFFFF) {
    speedR = 0xFFFF;
  } else if (speedR<0) {
    speedR = 0;
  }
  /* send new speed values to motor */
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
  MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
  MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
  {
    cnt++;
    if (cnt>10) { /* limit number of messages to the console */
      CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
      cnt = 0;

      CLS1_SendStr((unsigned char*)"line:", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" sum:", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" left:", ioOut);
      CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)" right:", ioOut);
      CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
      buf[0] = '\0';
      UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
      CLS1_SendStr(buf, ioOut);

      CLS1_SendStr((unsigned char*)"\r\n", ioOut);
    }
  }
#endif
}
Esempio n. 24
0
static void print(unsigned char *str) {
#if !ONLY_HOST
  CLS1_SendStr(str, CLS1_GetStdio()->stdOut);
#endif
}
Esempio n. 25
0
static uint8_t Tune(const CLS1_StdIOType *io) {
  uint8_t channel;
  uint16_t dac;
  int i;
  QuadTime_t timing;
  uint8_t buf[32];
  uint8_t res;
  uint16_t dacArr[4];
  uint8_t pd[4];
 
  MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 10);
  MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 10);
  for(i=0;i<4;i++) { /* init */
    dacArr[i] = 0;
    pd[i] = 0;
  }
  CLS1_SendStr((uint8_t*)"Tuning....\r\n", io->stdOut);
  res = ERR_OK;
  for(channel=0;channel<NOF_SIGNALS;channel++) {
    for(i=0,dac=0;i<80 && dac<MPC4728_MAX_DAC_VAL;i++,dac += 0x10) {
      UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
      UTIL1_strcatNum8u(buf, sizeof(buf), channel);
      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);
      dacArr[channel] = dac;
      if (MPC4728_FastWriteDAC(dacArr, sizeof(dacArr), pd, sizeof(pd))!=ERR_OK) {
        CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
        res = ERR_FAILED;
        break;
      }
      WAIT1_Waitms(50); /* wait some time to allow 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);
          break;
        }
      } else {
        CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
      }
    } /* for */
    if (res!=ERR_OK) {
      CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
      break;
    }
  } /* for */
  MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
  MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
  CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
  for (channel=0;channel<NOF_SIGNALS;channel++) {
    if (MPC4728_SingleWriteDACandEE(channel, dacArr[channel])!=ERR_OK) {
      CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
      res = ERR_FAILED;
      break;
    }
    WAIT1_Waitms(500); /* give EEPROM time to write data */
  }
  CLS1_SendStr((uint8_t*)"finished!\r\n", io->stdOut);
  return res;
}