You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
109 lines
6.6 KiB
109 lines
6.6 KiB
/*
|
|
* Copyright (c) 2019, Erich Styger
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#include "Identify.h"
|
|
#include "Motor.h"
|
|
#include "QuadCounter.h"
|
|
#include "buttons.h"
|
|
#include <stdbool.h>
|
|
|
|
typedef enum {
|
|
ROBOT_HW_V1,
|
|
ROBOT_HW_V2
|
|
} RobotHW_Version;
|
|
|
|
typedef struct {
|
|
ID_Robot_e id;
|
|
RobotHW_Version version;
|
|
bool invertDirLeft, invertDirRight;
|
|
bool swapQuadLeft, swapQuadRight;
|
|
} RobotHW_Config;
|
|
|
|
static const RobotHW_Config RobotHWConfigTable[] =
|
|
{ /*! \todo ADIS: The following table needs to be completed and verified for all robots */
|
|
{.id=ID_ROBOT_E0, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_E1, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_E2, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E3, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified*/
|
|
{.id=ID_ROBOT_E4, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E5, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E6, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=FALSE, .swapQuadLeft=TRUE,.swapQuadRight=TRUE }, /* verified */
|
|
{.id=ID_ROBOT_E7, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E8, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E9, .version=ROBOT_HW_V1, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=TRUE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_E10, .version=ROBOT_HW_V1, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=TRUE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_E12, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E13, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E14, .version=ROBOT_HW_V2, .invertDirLeft=true, .invertDirRight=false, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E17, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E18, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_E27, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
|
|
{.id=ID_ROBOT_L0, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_L1, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_L3, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_L17, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_L20, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
|
|
{.id=ID_ROBOT_R0, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R8, .version=ROBOT_HW_V1, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R9, .version=ROBOT_HW_V1, .invertDirLeft=FALSE, .invertDirRight=FALSE, .swapQuadLeft=TRUE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_R23, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R27, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R28, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R29, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R32, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R33, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_R34, .version=ROBOT_HW_V2, .invertDirLeft=FALSE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_R36, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
{.id=ID_ROBOT_R37, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_R44, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */
|
|
{.id=ID_ROBOT_R45, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE },
|
|
};
|
|
|
|
static const RobotHW_Config *GetRobotHWDesc(ID_Robot_e id) {
|
|
unsigned int i;
|
|
|
|
for(i=0; i<sizeof(RobotHWConfigTable)/sizeof(RobotHWConfigTable[0]); i++) {
|
|
if (RobotHWConfigTable[i].id == id) {
|
|
return &RobotHWConfigTable[i]; /* found it */
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
void ADOPT_AdoptToHardware(void) {
|
|
#if PL_CONFIG_USE_IDENTIFY
|
|
#if PL_CONFIG_USE_MOTORS
|
|
ID_Robot_e robot;
|
|
const RobotHW_Config *config;
|
|
|
|
robot = ID_WhichDevice();
|
|
if (robot!=ID_ROBOT_NONE) {
|
|
config = GetRobotHWDesc(robot);
|
|
if (config!=NULL) {
|
|
if (config->invertDirLeft) {
|
|
MOT_Invert(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TRUE); /* invert motor */
|
|
}
|
|
if (config->invertDirRight) {
|
|
MOT_Invert(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TRUE); /* invert motor */
|
|
}
|
|
if (config->swapQuadLeft) {
|
|
QuadCounter_SwapPinsLeft(true);
|
|
}
|
|
if (config->swapQuadRight) {
|
|
QuadCounter_SwapPinsRight(true);
|
|
}
|
|
}
|
|
if (config->version==ROBOT_HW_V2) {
|
|
BTN_EnablePullup(); /* enable internal pull-up for push button */
|
|
QuadCounter_EnablePullups(); /* pull-ups for Quadrature Encoder Pins */
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
}
|
|
|