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;
}
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;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, toggle remote*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
DRV_SetMode(DRV_MODE_SPEED);
} else if (val=='G') { /* center joystick button: horn*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 2000 300");
} else if (val=='A')
{
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(STRAIGHT_HAND);
LF_StartFollowing();
} else if (val=='C') { /* 'C' button: motor stop*/
DRV_SetMode(DRV_MODE_STOP);
} else if (val=='B') { /* 'B' button: start right-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(RIGHT_HAND);
LF_StartFollowing();
} else if (val=='D') { /* 'D' button: start left-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(LEFT_HAND);
LF_StartFollowing();
} else if (val=='E') {
REF_CalibrateStartStop();
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t direction, speed;
int16_t x1000, y1000;
*handled = TRUE;
direction = *data; /* get x data value */
speed = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
//x1000 = scaleJoystickTo1K(x);
//y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, disable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
REMOTE_SetOnOff(FALSE);
DRV_SetSpeed(0,0); /* turn off motors */
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
REMOTE_SetOnOff(TRUE);
DRV_SetMode(DRV_MODE_SPEED);
SHELL_SendString("Remote ON\r\n");
} else if (val=='C') { /* red 'C' button */
/*! \todo add functionality */
} else if (val=='A') { /* green 'A' button */
/*! \todo add functionality */
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
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;
}
void LF_StartFollowing(void) {
PID_Start();
LF_currState = STATE_FOLLOW_SEGMENT;
DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
}
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;
}