void setSMLenkung(void){
for(;;){
for(pos=0;pos<=10000;pos++){
WAIT1_WaitOSms(2);
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
WAIT1_WaitOSms(2);
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
LED_GREEN_Neg();
}
WAIT1_WaitOSms(2000);
Lenk_DIR_NegVal(Lenk_DIR_DeviceData);
for(pos;pos>=0;pos--){
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
WAIT1_WaitOSms(2);
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
WAIT1_WaitOSms(2);
}
Lenk_DIR_NegVal(Lenk_DIR_DeviceData);
}
}
/* Single Write DAC Input Register and EEPROM */
uint8_t MCP4728_WriteDACandEE(uint8_t channel, uint16_t val) {
uint8_t res;
uint8_t data[3];
/* 01011|DAC1|DAC0|UDAC VREF|PD1|PD0|Gx|D11-D0 */
if (channel>3) {
return ERR_FAILED; /* only channel 0-3 allowed */
}
data[0] = 0x58|((channel&0x3)<<1); /* UDAC zero */
data[1] = (uint8_t)((val>>8)&0x0F); /* VREF, PD1, PD2 and Gx zero */
data[2] = (uint8_t)(val&0xff); /* low byte */
res = GI2C1_SelectSlave(MCP4728_I2C_ADDRESS);
if (res!=ERR_OK) {
return res;
}
res = GI2C1_WriteBlock(&data[0], sizeof(data), GI2C1_SEND_STOP);
if (res!=ERR_OK) {
(void)GI2C1_UnselectSlave();
return res;
}
res = GI2C1_UnselectSlave();
if (res!=ERR_OK) {
return res;
}
#if PL_CONFIG_HAS_MCP4728_RDY
while(MCP4728_IsBusy()) {
WAIT1_WaitOSms(10); /* wait until data is written */
}
#else
WAIT1_WaitOSms(500); /* give EEPROM time to write data */
#endif
return ERR_OK;
}
/**
* \brief Touch Task. Starts and stops walking Task by touchsensor.
* @param TouchTask Taskname
* @param pvParameters Not used
* @return void
*/
static portTASK_FUNCTION(TouchTask, pvParameters) {
uint8_t errCode;
uint16_t touchStatus;
(void)pvParameters;
TOUCH_init();
WAIT1_WaitOSms(10);
for(;;) {
errCode=TOUCH_getELE(&touchStatus);
if(errCode != ERR_OK) {
for(;;) {} /* error occurred!*/
}
if(touchStatus!=0) {
if (enabledWalking) {
EnterCritical();
if(enabledWalking) { /* stop walking*/
enabledWalking = FALSE;
} else { /* start walking*/
enabledWalking = TRUE;
}
ExitCritical();
}
WAIT1_WaitOSms(1000);
}
WAIT1_WaitOSms(16); /* 16ms sampling time*/
}
}
static void Turn(bool isLeft, uint8_t duty, uint16_t val) {
#if PL_HAS_QUADRATURE
(void)duty;
if (isLeft) {
TURN_TurnSteps(-val, val);
} else {
TURN_TurnSteps(val, -val);
}
#else
if (isLeft) {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -duty);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), duty);
} else {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), duty);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -duty);
}
WAIT1_WaitOSms(val); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
}
void setWinkel(int steps){
for(steps;steps>0;steps--){
WAIT1_WaitOSms(WAITTIME);
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
WAIT1_WaitOSms(WAITTIME);
Lenk_STEP_NegVal(Lenk_STEP_DeviceData);
LED_GREEN_Neg();
}
}
uint8_t ESP_ReadCharsUntil(uint8_t *buf, size_t bufSize, uint8_t sentinelChar, uint16_t timeoutMs) {
uint8_t ch;
uint8_t res = ERR_OK;
if (bufSize<=1) {
return ERR_OVERRUN; /* buffer to small */
}
buf[0] = '\0'; buf[bufSize-1] = '\0'; /* always terminate */
bufSize--;
for(;;) { /* breaks */
if (bufSize==0) {
res = ERR_OVERRUN;
break;
}
if (AS2_GetCharsInRxBuf()>0) {
(void)AS2_RecvChar(&ch);
*buf = ch;
buf++;
bufSize--;
if (ch==sentinelChar) {
*buf = '\0'; /* terminate string */
break; /* sentinel found */
}
} else {
if (timeoutMs>10) {
WAIT1_WaitOSms(5);
timeoutMs -= 5;
} else {
res = ERR_NOTAVAIL; /* timeout */
break;
}
}
}
return res;
}
static uint8_t ReadIntoIPDBuffer(uint8_t *buf, size_t bufSize, uint8_t *p, uint16_t msgSize, uint16_t msTimeout, const CLS1_StdIOType *io) {
uint8_t ch;
size_t nofInBuf;
int timeout;
nofInBuf = p-buf;
bufSize -= nofInBuf; /* take into account what we already have in buffer */
timeout = msTimeout;
while (msgSize>0 && bufSize>0) {
if (AS2_GetCharsInRxBuf()>0) {
(void)AS2_RecvChar(&ch);
*p = ch;
if (io!=NULL) { /* copy on console */
io->stdOut(ch);
}
p++;
*p = '\0'; /* terminate */
nofInBuf++; msgSize--; bufSize--;
} else {
/* check in case we recveive less characters than expected, happens for POST? */
if (nofInBuf>6 && UTIL1_strncmp(&p[-6], "\r\nOK\r\n", sizeof("\r\nOK\r\n")-1)==0) {
break;
} else {
timeout -= 10;
WAIT1_WaitOSms(10);
if (timeout<0) {
return ERR_BUSY;
}
}
}
}
return ERR_OK;
}
static void AutoCalibrateReflectance(const CLS1_StdIOType *io) {
CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", io->stdOut);
/* perform automatic calibration */
APP_StateStartCalibrate(io);
TURN_Turn(TURN_LEFT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_RIGHT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_RIGHT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_LEFT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_STOP);
APP_StateStopCalibrate(io);
CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", io->stdOut);
}
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 += 0x10; /* 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;
}
/* measure and return the microseconds */
uint16_t US_Measure_us(void) {
int timeout;
FRTOS1_xSemaphoreTake(mutexHandle, portMAX_DELAY);
usDevice.signalState = US_SIGNAL_OK; /* default state */
/* send 10us pulse on TRIG line. */
TRIG_SetVal(usDevice.trigDevice);
WAIT1_Waitus(10);
usDevice.state = ECHO_TRIGGERED;
TU_US_Enable(usDevice.echoDevice);
TRIG_ClrVal(usDevice.trigDevice);
timeout = 30; /* timout in ms */
while(usDevice.state!=ECHO_FINISHED) {
/* measure echo pulse */
if (usDevice.state==ECHO_OVERFLOW) { /* measurement took too long? */
usDevice.signalState = US_SIGNAL_NO_ECHO;
break; /* no echo, error case */
}
WAIT1_WaitOSms(1);
timeout--;
if (timeout==0) {
usDevice.signalState = US_SIGNAL_TIMEOUT;
break;
}
} /* while */
if (usDevice.state==ECHO_FINISHED) {
usDevice.lastValue_us = (usDevice.capture*1000UL)/(TU_US_CNT_INP_FREQ_U_0/1000);
usDevice.signalState = US_SIGNAL_OK;
} else {
usDevice.lastValue_us = 0; /* error case */
}
usDevice.state = ECHO_IDLE;
FRTOS1_xSemaphoreGive(mutexHandle);
return usDevice.lastValue_us;
}
/* measure and return the microseconds */
uint16_t US_Measure_us(void) {
int timeout;
FRTOS1_xSemaphoreTake(mutexHandle, portMAX_DELAY);
/* send 10us pulse on TRIG line. */
TRIG_SetVal(usDevice.trigDevice);
WAIT1_Waitus(10);
usDevice.state = ECHO_TRIGGERED;
TRIG_ClrVal(usDevice.trigDevice);
timeout = 30;
while(usDevice.state!=ECHO_FINISHED && timeout>0) {
/* measure echo pulse */
if (usDevice.state==ECHO_OVERFLOW) { /* measurement took too long? */
usDevice.state = ECHO_IDLE; /* go back to idle */
break; /* no echo, error case */
}
WAIT1_WaitOSms(1);
timeout--;
} /* while */
if (usDevice.state==ECHO_FINISHED) {
usDevice.lastValue_us = (usDevice.capture*1000UL)/(TU_US_CNT_INP_FREQ_U_0/1000);
} else {
usDevice.lastValue_us = 0; /* error case */
}
usDevice.state = ECHO_IDLE;
FRTOS1_xSemaphoreGive(mutexHandle);
return usDevice.lastValue_us;
}
static void CheckButton(void) {
uint32_t timeTicks; /* time in ticks */
#define BUTTON_CNT_MS 100 /* iteration count for button */
bool autoCalibrate = FALSE;
if (SW1_GetVal()==0) { /* button pressed */
/* short press (1 beep): start or stop line following if calibrated
* 1 s press (2 beep): calibrate manually
* 2 s press (3 beep): calibrate with auto-move
* 3 s press (4 beep or more): clear path
* */
FRTOS1_vTaskDelay(50/portTICK_RATE_MS); /* simple debounce */
if (SW1_GetVal()==0) { /* still pressed */
LEDG_On();
timeTicks = 0;
while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) {
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
if ((timeTicks%(1000/BUTTON_CNT_MS))==0) {
#if PL_HAS_BUZZER
BUZ_Beep(300, 200);
#endif
}
timeTicks++;
} /* wait until released */
autoCalibrate = FALSE;
if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */
CLS1_SendStr((unsigned char*)"button press.\r\n", CLS1_GetStdio()->stdOut);
StateMachine(TRUE); /* <1 s, short button press, according to state machine */
} else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */
} else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"auto calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 2-3 s: start auto calibration */
autoCalibrate = TRUE;
} else if (timeTicks>=(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"delete solution.\r\n", CLS1_GetStdio()->stdOut);
MAZE_ClearSolution();
}
while (SW1_GetVal()==0) { /* wait until button is released */
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
}
if (autoCalibrate) {
CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", CLS1_GetStdio()->stdOut);
/* perform automatic calibration */
WAIT1_WaitOSms(1500); /* wait some time */
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_STOP);
APP_StateStopCalibrate();
CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", CLS1_GetStdio()->stdOut);
}
}
} /* if */
}
static portTASK_FUNCTION(WalkingTask, pvParameters) {
ROBO_State state=ROBO_ROTATE_HIP_LEFT; /* state variable for FSM */
int16_t ms=500; /* global wait variable for change walking speed*/
(void)pvParameters; /* parameter not used */
for(;;) {
if (!enabledWalking) {
WAIT1_WaitOSms((uint16_t)1000);
} else {
switch(state) {
case ROBO_LEAN_RIGHT: /* lean right*/
LFEET_MovePos(40,ms);
RFEET_MovePos(110,ms);
WAIT1_WaitOSms((uint16_t)ms);
RFEET_MovePos(100,ms);
WAIT1_WaitOSms((uint16_t)ms);
state=ROBO_ROTATE_HIP_RIGHT;
break;
case ROBO_ROTATE_HIP_RIGHT: /* rotate hips*/
LFEET_MovePos(127,ms);
RHIP_MovePos(200,ms-200);
LHIP_MovePos(200,ms-200);
WAIT1_WaitOSms((uint16_t)ms);
RFEET_MovePos(127,ms+200);/* initPos*/
WAIT1_WaitOSms((uint16_t)(ms+200));
state=ROBO_LEAN_LEFT;
break;
case ROBO_LEAN_LEFT: /* lean left*/
RFEET_MovePos(255,ms);
LFEET_MovePos(170,ms);
WAIT1_WaitOSms((uint16_t)(ms));
LFEET_MovePos(180,ms);
WAIT1_WaitOSms((uint16_t)(ms));
state=ROBO_ROTATE_HIP_LEFT;
break;
case ROBO_ROTATE_HIP_LEFT: /* rotate hips*/
RFEET_MovePos(127,ms);
RHIP_MovePos(50,ms-200);
LHIP_MovePos(50,ms-200);
WAIT1_WaitOSms((uint16_t)(ms));
LFEET_MovePos(127,ms+200);/* initPos*/
WAIT1_WaitOSms((uint16_t)(ms+200));
state=ROBO_LEAN_RIGHT;
break;
} /* switch */
} /* if */
} /* for */
}
uint16_t SOCK_Send(uint8_t sock, const uint8_t *buf, uint16_t buflen) {
uint16_t offaddr, realaddr, txsize, timeout;
uint8_t val;
if (buflen<=0 || sock!=0) {
return 0;
}
#if _DEBUG_MODE
printf("Send Size: %d\n",buflen);
#endif
/* Make sure the TX Free Size Register is available */
W5100_MemReadWord(W5100_SO_TX_FSR, &txsize);
#if _DEBUG_MODE
printf("TX Free Size: %d\n",txsize);
#endif
timeout=0;
while (txsize<buflen) {
WAIT1_WaitOSms(1);
W5100_MemReadWord(W5100_SO_TX_FSR, &txsize);
/* Timeout for approximately 1000 ms */
if (timeout++ > 1000) {
#if _DEBUG_MODE
printf("TX Free Size Error!\n");
#endif
/* Disconnect the connection */
SOCK_Disconnect(sock);
return 0;
}
}
/* Read the Tx Write Pointer */
W5100_MemReadWord(W5100_S0_TX_WR, &offaddr);
#if _DEBUG_MODE
printf("TX Buffer: %x\n", offaddr);
#endif
while(buflen) {
buflen--;
/* Calculate the real W5100 physical Tx Buffer Address */
realaddr = W5100_TXBUFADDR+(offaddr&W5100_TX_BUF_MASK);
/* Copy the application data to the W5100 Tx Buffer */
W5100_MemWriteByte(realaddr, *buf);
offaddr++;
buf++;
}
/* Increase the S0_TX_WR value, so it point to the next transmit */
W5100_MemWriteWord(W5100_S0_TX_WR, offaddr);
/* Now Send the SEND command */
W5100_MemWriteByte(W5100_S0_CR, W5100_CR_SEND);
/* Wait for Sending Process */
do {
W5100_MemReadByte(W5100_S0_CR, &val);
} while(val!=0);
return 1;
}
/*
** ===================================================================
** Method : CLS1_SendChar (component Shell)
** Description :
** Sends a character (blocking)
** Parameters :
** NAME - DESCRIPTION
** ch - character to be sent
** Returns : Nothing
** ===================================================================
*/
void CLS1_SendChar(uint8_t ch)
{
uint8_t res;
do {
res = AS1_SendChar((uint8_t)ch); /* Send char */
if (res==ERR_TXFULL) {
WAIT1_WaitOSms(10);
}
} while(res==ERR_TXFULL);
}
static void CheckKey(void) {
uint16_t cnt;
uint8_t data;
if (SW1_GetVal()==0) { /* low active */
WAIT1_WaitOSms(50); /* simple debounce */
if (SW1_GetVal()==0) { /* still pressed? */
cnt = 0;
while(SW1_GetVal()==0) { /* wait until released */
WAIT1_WaitOSms(10);
cnt += 10;
}
if (cnt>1000) { /* long press */
data = 2; /* toggle LED2 */
} else {
data = 1; /* toggle LED1 */
}
(void)RAPP_SendPayloadDataBlock(&data, sizeof(data), RAPP_MSG_TYPE_DATA, RNWK_ADDR_BROADCAST, RPHY_PACKET_FLAGS_NONE);
}
}
}
uint8_t ESP_JoinAP(const uint8_t *ssid, const uint8_t *pwd, int nofRetries, CLS1_ConstStdIOType *io) {
uint8_t buf[32];
uint8_t res;
do {
res = JoinAccessPoint(ssid, pwd, io);
if (res==ERR_OK) {
break;
}
WAIT1_WaitOSms(1000);
nofRetries--;
} while (nofRetries>0);
return res;
}
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;
}
void TestClock(void) {
int hour, minute, second;
for(;;) {
for(hour=0;hour<24;hour++) {
for(minute=0;minute<60;minute++) {
for(second=0;second<60;second++) {
CLOCK_SetTime(hour, minute, second);
(void)NEO_TransferPixels();
WAIT1_WaitOSms(1);
}
}
}
}
}
/*
** ===================================================================
** Method : RTT1_StdIOSendChar (component SeggerRTT)
** Description :
** StdIO handler to sends a character.
** Parameters :
** NAME - DESCRIPTION
** ch - Character to send
** Returns : Nothing
** ===================================================================
*/
void RTT1_StdIOSendChar(uint8_t ch)
{
int timeoutMs = 5;
for(;;) { /* will break */
if (RTT1_Write(0, (const char*)&ch, 1)==1) { /* non blocking send, check that we were able to send */
break; /* was able to send character, get out of waiting loop */
}
WAIT1_WaitOSms(1);
if(timeoutMs<=0) {
break; /* timeout */
}
timeoutMs -= 1;
} /* for */
}
static uint8_t RxResponse(unsigned char *rxBuf, size_t rxBufLength, unsigned char *expectedTail, uint16_t msTimeout) {
unsigned char ch;
uint8_t res = ERR_OK;
unsigned char *p;
if (rxBufLength < sizeof("x\r\n")) {
return ERR_OVERFLOW; /* not enough space in buffer */
}
p = rxBuf;
p[0] = '\0';
for(;;) { /* breaks */
if (msTimeout == 0) {
break; /* will decide outside of loop if it is a timeout. */
} else if (rxBufLength == 0) {
res = ERR_OVERFLOW; /* not enough space in buffer */
break;
} else if (AS2_GetCharsInRxBuf() > 0) {
#if 0
if (AS2_RecvChar(&ch) != ERR_OK) {
res = ERR_RXEMPTY;
break;
}
#else
/* might get an overrun OVERRUN_ERR error here? Ignoring error for now */
(void)AS2_RecvChar(&ch);
#endif
*p++ = ch;
*p = '\0'; /* always terminate */
rxBufLength--;
} else if (expectedTail!=NULL && expectedTail[0]!='\0'
&& UTIL1_strtailcmp(rxBuf, expectedTail) == 0) {
break; /* finished */
} else {
WAIT1_WaitOSms(1);
msTimeout--;
}
} /* for */
if (msTimeout==0) { /* timeout! */
if (expectedTail[0] != '\0' /* timeout, and we expected something: an error for sure */
|| rxBuf[0] == '\0' /* timeout, did not know what to expect, but received nothing? There has to be a response. */
)
{
res = ERR_FAULT;
}
}
return res;
}
/*
** ===================================================================
** Method : CLS1_SendChar (component Shell)
** Description :
** Sends a character (blocking)
** Parameters :
** NAME - DESCRIPTION
** ch - character to be sent
** Returns : Nothing
** ===================================================================
*/
void CLS1_SendChar(uint8_t ch)
{
uint8_t res;
int timeoutMs = 20;
do {
res = AS1_SendChar((uint8_t)ch); /* Send char */
if (res==ERR_TXFULL) {
WAIT1_WaitOSms(10);
}
if(timeoutMs<=0) {
break; /* timeout */
}
timeoutMs -= 10;
} while(res==ERR_TXFULL);
}
/**
* \brief AccelToLED Task. Represents acceleration of x-axis with LED's (like a water-level).
* @param AccelToLEDTask Taskname
* @param pvParameters Not used
* @return void
*/
static portTASK_FUNCTION(AccelToLEDTask, pvParameters) {
int16_t accelX;
uint8_t ledNumOld = 4;
uint8_t ledNumNew;
(void)pvParameters;
EnterCritical();
PCA1_WriteI2CByte(PCA1_ADDR_REG_CONFIG, 0x00); /* all as output*/
PCA1_ClearOutputBit(1);
PCA1_ClearOutputBit(2);
PCA1_ClearOutputBit(3);
PCA1_SetOutputBit(4); /* activate LED in the middle*/
PCA1_ClearOutputBit(5);
PCA1_ClearOutputBit(6);
PCA1_ClearOutputBit(7);
/* Don't write to Pin0 of IO-Expander1! (flash-CS)*/
ACCEL_Init();
if(ACCEL_CalibrateAxes() != ERR_OK) {
for(;;) {} /* error occurred*/
}
ExitCritical();
for(;;) {
WAIT1_WaitOSms(50);
EnterCritical();
if(ACCEL_GetXmg(&accelX) != ERR_OK) {
for(;;) {} /* error occurred!*/
}
ExitCritical();
/* scale accelX (-1000..1000) to LEDs (IO-Exp Pin 1..7)*/
ledNumNew = (uint8_t)(((accelX+1000)*7/2000)+1);
if(ledNumNew < 1) {
ledNumNew = 1;
} else if(ledNumNew > 7) {
ledNumNew = 7;
}
if(ledNumNew != ledNumOld) { /* change LED*/
EnterCritical();
PCA1_ClearOutputBit(ledNumOld);
PCA1_SetOutputBit(ledNumNew);
ExitCritical();
}
ledNumOld = ledNumNew;
}
}
uint8_t SOCK_Send(uint8_t sock, const uint8_t *buf, size_t buflen) {
uint16_t offaddr, realaddr, txsize, timeout, sockaddr;
uint8_t val;
if (buflen<=0 || sock>=W5100_NUM_SOCKETS) {
return ERR_VALUE;
}
sockaddr = W5100_SKT_BASE(sock);
/* Make sure the TX Free Size Register is available */
W5100_MemReadWord(sockaddr+W5100_TX_FSR_OFFSET, &txsize);
timeout=0;
while (txsize<buflen) {
WAIT1_WaitOSms(1);
W5100_MemReadWord(sockaddr+W5100_TX_FSR_OFFSET, &txsize);
/* Timeout for approximately 1000 ms */
if (timeout++ > 1000) {
/* Disconnect the connection */
SOCK_Disconnect(sock);
return ERR_FAILED;
}
}
/* Read the Tx Write Pointer */
W5100_MemReadWord(sockaddr+W5100_TX_WR_OFFSET, &offaddr);
while(buflen) {
buflen--;
/* Calculate the real W5100 physical Tx Buffer Address */
realaddr = (W5100_TXBUFADDR+(0x0800*sock)) + (offaddr&W5100_TX_BUF_MASK);
/* Copy the application data to the W5100 Tx Buffer */
W5100_MemWriteByte(realaddr, *buf);
offaddr++;
buf++;
}
/* Increase the S0_TX_WR value, so it point to the next transmit */
W5100_MemWriteWord(sockaddr+W5100_TX_WR_OFFSET, offaddr);
/* Now Send the SEND command */
W5100_MemWriteByte(sockaddr+W5100_CR_OFFSET, W5100_CR_SEND);
/* Wait for Sending Process */
do {
W5100_MemReadByte(sockaddr+W5100_CR_OFFSET, &val);
} while(val!=0);
return ERR_OK;
}
uint8_t ESP_SendStr(const uint8_t *str, CLS1_ConstStdIOType *io) {
uint8_t buf[32];
uint8_t rxBuf[48];
uint8_t res;
uint16_t timeoutMs;
#define RX_TIMEOUT_MS 3000
AS2_TComData ch;
UTIL1_strcpy(buf, sizeof(buf), str);
UTIL1_strcat(buf, sizeof(buf), "\r\n");
res = ESP_SendATCommand(buf, rxBuf, sizeof(rxBuf), NULL, ESP_DEFAULT_TIMEOUT_MS, io);
timeoutMs = 0;
while(timeoutMs<RX_TIMEOUT_MS) {
WAIT1_WaitOSms(100);
timeoutMs += 100;
while (AS2_GetCharsInRxBuf()>0) {
(void)AS2_RecvChar(&ch);
CLS1_SendChar(ch);
}
}
return ERR_OK;
}
void TURN_TurnSteps(int16_t stepsL, int16_t stepsR) {
int16_t currLPos, currRPos, targetLPos, targetRPos;
uint8_t matchCntr = 100;
currLPos = Q4CLeft_GetPos();
targetLPos = currLPos+stepsL;
currRPos = Q4CRight_GetPos();
targetRPos = currRPos+stepsR;
for(;;) { /* breaks */
if (match(currLPos,targetLPos) && match(currRPos,targetRPos)) {
matchCntr--;
if (matchCntr==0) {
break; /* get out of loop */
}
}
PID_Pos(Q4CLeft_GetPos(), targetLPos, TRUE);
PID_Pos(Q4CRight_GetPos(), targetRPos, FALSE);
WAIT1_WaitOSms(2);
currLPos = Q4CLeft_GetPos();
currRPos = Q4CRight_GetPos();
} /* for */
}
/*!
* \brief Radio power-on initialization.
* \return Error code, ERR_OK if everything is ok.
*/
uint8_t RADIO_PowerUp(void) {
WAIT1_WaitOSms(100); /* the transceiver needs 100 ms power up time */
RF1_Init(); /* set CE and CSN to initialization value */
RF1_WriteRegister(RF1_RF_SETUP, RF1_RF_SETUP_RF_PWR_0|RF1_RF_SETUP_RF_DR_250);
#if NRF24_DYNAMIC_PAYLOAD
/* enable dynamic payload */
RF1_WriteFeature(RF1_FEATURE_EN_DPL|RF1_FEATURE_EN_ACK_PAY|RF1_FEATURE_EN_DYN_PAY); /* set EN_DPL for dynamic payload */
RF1_EnableDynanicPayloadLength(RF1_DYNPD_DPL_P0); /* set DYNPD register for dynamic payload for pipe0 */
#else
RF1_SetStaticPipePayload(0, RPHY_PAYLOAD_SIZE); /* static number of payload bytes we want to send and receive */
#endif
(void)RADIO_SetChannel(RADIO_CHANNEL_DEFAULT);
/* Set RADDR and TADDR as the transmit address since we also enable auto acknowledgment */
RF1_WriteRegisterData(RF1_RX_ADDR_P0, (uint8_t*)TADDR, sizeof(TADDR));
RF1_WriteRegisterData(RF1_TX_ADDR, (uint8_t*)TADDR, sizeof(TADDR));
/* Enable RX_ADDR_P0 address matching */
RF1_WriteRegister(RF1_EN_RXADDR, RF1_EN_RXADDR_ERX_P0); /* enable data pipe 0 */
/* clear interrupt flags */
RF1_ResetStatusIRQ(RF1_STATUS_RX_DR|RF1_STATUS_TX_DS|RF1_STATUS_MAX_RT);
/* rx/tx mode */
RF1_EnableAutoAck(RF1_EN_AA_ENAA_P0); /* enable auto acknowledge on pipe 0. RX_ADDR_P0 needs to be equal to TX_ADDR! */
RF1_WriteRegister(RF1_SETUP_RETR, RF1_SETUP_RETR_ARD_750|RF1_SETUP_RETR_ARC_15); /* Important: need 750 us delay between every retry */
RX_POWERUP(); /* Power up in receiving mode */
RF1_StartRxTx(); /* Listening for packets */
RADIO_AppStatus = RADIO_INITIAL_STATE;
/* init Rx descriptor */
radioRx.phyData = &radioRxBuf[0];
radioRx.phySize = sizeof(radioRxBuf);
radioRx.rxtx = &RPHY_BUF_SIZE(radioRx.phyData); /* we transmit the size too */
return ERR_OK;
}
/*
** ===================================================================
** Method : EE241_WriteByte (component 24AA_EEPROM)
** Description :
** Writes a single byte to specified address
** Parameters :
** NAME - DESCRIPTION
** addr - The address inside the EEPROM
** data - The data value to write
** Returns :
** --- - Error code, possible values
** ERR_OK - OK
** otherwise it can return an error code of
** the underlying communication protocol.
** ===================================================================
*/
byte EE241_WriteByte(EE241_Address addr, byte data)
{
uint8_t res, block[3];
res = GI2C1_SelectSlave(EE241_DEVICE_ADDR(addr));
if (res != ERR_OK) {
(void)GI2C1_UnselectSlave();
return res;
}
#if EE241_DEVICE_ID==EE241_DEVICE_ID_8
block[0] = (uint8_t)(addr&0xff); /* low byte of address */
block[1] = data; /* switch to read mode */
res = GI2C1_WriteBlock(block, 2, GI2C1_SEND_STOP); /* send address and data */
#else
block[0] = (uint8_t)(addr>>8); /* high byte of address */
block[1] = (uint8_t)(addr&0xff); /* low byte of address */
block[2] = data; /* switch to read mode */
res = GI2C1_WriteBlock(block, sizeof(block), GI2C1_SEND_STOP); /* send address and data */
#endif
if (res != ERR_OK) {
(void)GI2C1_UnselectSlave();
return res;
}
#if EE241_DO_ACKNOWLEDGE_POLLING
/* do acknowledge polling */
block[0] = 0xff; /* dummy value */
do {
WAIT1_WaitOSms(EE241_PAGE_WRITE_TIME_MS);
res = GI2C1_ProbeACK(block, 1, GI2C1_SEND_STOP, EE241_ACK_POLLING_TIME_US); /* send address and data */
} while(res!=ERR_OK); /* wait until we get an ACK */
#endif /* EE241_DO_ACKNOWLEDGE_POLLING */
if (res != ERR_OK) {
(void)GI2C1_UnselectSlave();
return res;
}
return GI2C1_UnselectSlave();
}
/**
* \brief Accel Task. Tests the acceleration sensor, by writing its accelvalues to the shell.
* @param AccelTask Taskname
* @param pvParameters Not used
* @return void
*/
static portTASK_FUNCTION(AccelTask, pvParameters) {
int16_t accel[] = {0,0,0};
uint8_t errCode;
(void)pvParameters;
ACCEL_Init();
if(ACCEL_CalibrateAxes()!= ERR_OK) {
for(;;) {} /* error occurred!*/
}
for(;;) {
WAIT1_WaitOSms(200);
errCode = ACCEL_GetXYZmg(accel);
if(errCode != ERR_OK) {
for(;;) {} /* error occurred!*/
}
CLS1_SendStr((const unsigned char*)"X: ",CLS1_GetStdio()->stdOut);
CLS1_SendNum16s(accel[0], CLS1_GetStdio()->stdOut);
CLS1_SendStr((const unsigned char*)"\tY: ",CLS1_GetStdio()->stdOut);
CLS1_SendNum16s(accel[1], CLS1_GetStdio()->stdOut);
CLS1_SendStr((const unsigned char*)"\tZ: ",CLS1_GetStdio()->stdOut);
CLS1_SendNum16s(accel[2], CLS1_GetStdio()->stdOut);
CLS1_SendStr((const unsigned char*)"\r\n",CLS1_GetStdio()->stdOut);
}
}
static void StateMachine(void) {
switch (LF_currState) {
case STATE_IDLE:
break;
case STATE_FOLLOW_SEGMENT:
if (!FollowSegment(LINE_FOLLOW_FW)) {
#if PL_APP_LINE_MAZE
LF_currState = STATE_TURN; /* make turn */
#else
LF_currState = STATE_STOP; /* stop if we do not have a line any more */
#endif
}
break;
#if PL_APP_LINE_MAZE
case STATE_FOLLOW_SEGMENT_BW:
if (!FollowSegment(FALSE)) {
TURN_Turn(TURN_STOP);
if (EvaluateTurnBw()==ERR_OK) {
LF_currState = STATE_FOLLOW_SEGMENT;
} else {
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_TURN:
if (MAZE_IsSolved()) {
TURN_Kind turn;
turn = MAZE_GetSolvedTurn(&LF_solvedIdx);
if (turn==TURN_STOP) { /* last turn reached */
TURN_Turn(turn);
LF_currState = STATE_FINISHED;
} else { /* perform turning */
TURN_Turn(TURN_STEP_LINE_FW); /* Step over line */
TURN_Turn(TURN_STEP_POST_LINE_FW); /* step before doing the turn */
TURN_Turn(turn);
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* still evaluating maze */
bool deadEndGoBw = FALSE;
bool finished = FALSE;
if (EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */
if (finished) {
LF_currState = STATE_FINISHED;
MAZE_SetSolved();
#if PL_TURN_ON_FINISH
/* turn the robot */
TURN_Turn(TURN_LEFT180);
#endif
TURN_Turn(TURN_STOP);
/* now ready to do line following */
} else if (deadEndGoBw) {
LF_currState = STATE_FOLLOW_SEGMENT_BW;
} else {
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* error case */
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_APP_LINE_MAZE
case STATE_FINISHED:
#if PL_HAS_BUZZER
{
uint8_t i;
for(i=0;i<4;i++) {
(void)BUZ_Beep(300, 100);
WAIT1_WaitOSms(500);
}
}
#endif
LF_currState = STATE_STOP;
break;
#endif
case STATE_STOP:
TURN_Turn(TURN_STOP);
LF_currState = STATE_IDLE;
break;
} /* switch */
}
