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.
412 lines
14 KiB
412 lines
14 KiB
/**
|
|
* \file
|
|
* \brief Motor driver implementation.
|
|
* \author Erich Styger, erich.styger@hslu.ch
|
|
* \license SPDX-License-Identifier: BSD-3-Clause
|
|
* This module a driver for up to two small DC motors.
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#if PL_CONFIG_USE_MOTORS
|
|
#include "Motor.h"
|
|
#include "McuGPIO.h"
|
|
#include "Timer.h"
|
|
#include "McuUtility.h"
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
#include "BrakeL.h"
|
|
#include "BrakeR.h"
|
|
#endif
|
|
#if PL_HAS_MOTOR_CURRENT_SENSE
|
|
#include "SNS0.h"
|
|
#endif
|
|
#if PL_HAS_MOTOR_INAB
|
|
#include "DIRRB.h"
|
|
#include "DIRLB.h"
|
|
#endif
|
|
|
|
static MOT_MotorDevice motorL, motorR;
|
|
static bool isMotorOn = TRUE;
|
|
static McuGPIO_Handle_t MOT_DirL, MOT_DirR;
|
|
|
|
MOT_MotorDevice *MOT_GetMotorHandle(MOT_MotorSide side) {
|
|
if (side==MOT_MOTOR_LEFT) {
|
|
return &motorL;
|
|
} else {
|
|
return &motorR;
|
|
}
|
|
}
|
|
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
void MOT_SetBrake(MOT_MotorDevice *motor, bool on) {
|
|
if (on && !motor->brake) {
|
|
motor->BrakePutVal(1);
|
|
motor->brake = TRUE;
|
|
} else if (!on && motor->brake) {
|
|
motor->BrakePutVal(0);
|
|
motor->brake = FALSE;
|
|
}
|
|
}
|
|
|
|
bool MOT_GetBrake(MOT_MotorDevice *motor) {
|
|
return motor->brake;
|
|
}
|
|
#endif
|
|
|
|
#if PL_HAS_MOTOR_CURRENT_SENSE
|
|
void MOT_MeasureCurrent(MOT_MotorDevice *motorA, MOT_MotorDevice *motorB) {
|
|
#define SAMPLE_GROUP_SIZE 1U
|
|
SNS0_TResultData MeasuredValues[SAMPLE_GROUP_SIZE];
|
|
LDD_ADC_TSample SampleGroup[SAMPLE_GROUP_SIZE];
|
|
|
|
SampleGroup[0].ChannelIdx = 0U; /* Create one-sample group */
|
|
(void)SNS0_CreateSampleGroup(motorA->SNSdeviceData, (LDD_ADC_TSample *)SampleGroup, SAMPLE_GROUP_SIZE); /* Set created sample group */
|
|
(void)SNS0_StartSingleMeasurement(motorA->SNSdeviceData); /* Start continuous measurement */
|
|
while (!SNS0_GetMeasurementCompleteStatus(motorA->SNSdeviceData)) {}; /* Wait for conversion completeness */
|
|
(void)SNS0_GetMeasuredValues(motorA->SNSdeviceData, (LDD_TData *)MeasuredValues); /* Read measured values */
|
|
motorA->currentValue = MeasuredValues[0];
|
|
|
|
SampleGroup[0].ChannelIdx = 1U; /* Create one-sample group */
|
|
(void)SNS0_CreateSampleGroup(motorB->SNSdeviceData, (LDD_ADC_TSample *)SampleGroup, SAMPLE_GROUP_SIZE); /* Set created sample group */
|
|
(void)SNS0_StartSingleMeasurement(motorB->SNSdeviceData); /* Start continuous measurement */
|
|
while (!SNS0_GetMeasurementCompleteStatus(motorB->SNSdeviceData)) {}; /* Wait for conversion completeness */
|
|
(void)SNS0_GetMeasuredValues(motorB->SNSdeviceData, (LDD_TData *)MeasuredValues); /* Read measured values */
|
|
motorB->currentValue = MeasuredValues[0];
|
|
}
|
|
|
|
static void WriteCurrent(MOT_MotorDevice *motor, const McuShell_StdIOType *io) {
|
|
unsigned char buf[26];
|
|
uint16_t val;
|
|
|
|
buf[0] = '\0';
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", current val 0x");
|
|
McuUtility_strcatNum16Hex(buf, sizeof(buf), motor->currentValue);
|
|
McuShell_SendStr(buf, io->stdOut);
|
|
|
|
val = motor->currentValue/(0xFFFF/3300); /* 3300 mV full scale */
|
|
buf[0] = '\0';
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", mV ");
|
|
McuUtility_strcatNum16u(buf, sizeof(buf), val);
|
|
McuShell_SendStr(buf, io->stdOut);
|
|
|
|
val = motor->currentValue/(0xFFFF/2000); /* 2000 mA full scale */
|
|
buf[0] = '\0';
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)", mA ");
|
|
McuUtility_strcatNum16u(buf, sizeof(buf), val);
|
|
McuShell_SendStr(buf, io->stdOut);
|
|
}
|
|
#endif
|
|
|
|
static void PWMLSetRatio16(uint16_t ratio) {
|
|
/* lower ratio is higher speed */
|
|
ratio = 0xffff-ratio; /* because already inverted */
|
|
ratio = ratio/(0xffff/100); /* scale to 0...100 */
|
|
TMR_SetLeftMotorPWMDutyPercent(ratio);
|
|
}
|
|
|
|
static void PWMRSetRatio16(uint16_t ratio) {
|
|
ratio = 0xffff-ratio; /* because already inverted */
|
|
ratio = ratio/(0xffff/100); /* scale to 0...100 */
|
|
TMR_SetRightMotorPWMDutyPercent(ratio);
|
|
}
|
|
|
|
static void DirLPutVal(bool val) {
|
|
McuGPIO_SetValue(MOT_DirL, val);
|
|
}
|
|
|
|
static void DirRPutVal(bool val) {
|
|
McuGPIO_SetValue(MOT_DirR, val);
|
|
}
|
|
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
static void BrakeLPutVal(bool val) {
|
|
BrakeL_PutVal(val);
|
|
}
|
|
|
|
static void BrakeRPutVal(bool val) {
|
|
BrakeR_PutVal(val);
|
|
}
|
|
#endif /* PL_HAS_MOTOR_BRAKE */
|
|
|
|
void MOT_SetVal(MOT_MotorDevice *motor, uint16_t val) {
|
|
if (isMotorOn) {
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
if (val==0xffff) { /* stop */
|
|
motor->currPWMvalue = 0;
|
|
motor->SetRatio16(0); /* for max brake */
|
|
MOT_SetBrake(motor, TRUE);
|
|
} else {
|
|
motor->currPWMvalue = val;
|
|
MOT_SetBrake(motor, FALSE);
|
|
motor->SetRatio16(val);
|
|
}
|
|
#else
|
|
motor->currPWMvalue = val;
|
|
(void)motor->SetRatio16(val);
|
|
#endif
|
|
} else { /* have motor stopped */
|
|
motor->currPWMvalue = 0xFFFF;
|
|
(void)motor->SetRatio16(0xFFFF);
|
|
}
|
|
}
|
|
|
|
void MOT_OnOff(bool on) {
|
|
isMotorOn = on;
|
|
if (!on) {
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
|
|
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
|
|
}
|
|
}
|
|
|
|
uint16_t MOT_GetVal(MOT_MotorDevice *motor) {
|
|
return motor->currPWMvalue;
|
|
}
|
|
|
|
#if MOTOR_HAS_INVERT
|
|
void MOT_Invert(MOT_MotorDevice *motor, bool inverted) {
|
|
motor->inverted = inverted;
|
|
}
|
|
#endif
|
|
|
|
|
|
void MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
|
|
uint32_t val;
|
|
|
|
if (percent>100) { /* make sure we are within 0..100 */
|
|
percent = 100;
|
|
} else if (percent<-100) {
|
|
percent = -100;
|
|
}
|
|
motor->currSpeedPercent = percent; /* store value */
|
|
if (percent<0) {
|
|
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
|
|
percent = -percent; /* make it positive */
|
|
} else {
|
|
MOT_SetDirection(motor, MOT_DIR_FORWARD);
|
|
}
|
|
val = ((100-percent)*0xffff)/100; /* H-Bridge is low active! */
|
|
MOT_SetVal(motor, (uint16_t)val);
|
|
}
|
|
|
|
void MOT_UpdatePercent(MOT_MotorDevice *motor, MOT_Direction dir) {
|
|
motor->currSpeedPercent = ((0xffff-motor->currPWMvalue)*100)/0xffff;
|
|
if (dir==MOT_DIR_BACKWARD) {
|
|
motor->currSpeedPercent = -motor->currSpeedPercent;
|
|
}
|
|
}
|
|
|
|
MOT_SpeedPercent MOT_GetSpeedPercent(MOT_MotorDevice *motor) {
|
|
return motor->currSpeedPercent;
|
|
}
|
|
|
|
void MOT_ChangeSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent relPercent) {
|
|
relPercent += motor->currSpeedPercent; /* make absolute number */
|
|
if (relPercent>100) { /* check for overflow */
|
|
relPercent = 100;
|
|
} else if (relPercent<-100) { /* and underflow */
|
|
relPercent = -100;
|
|
}
|
|
MOT_SetSpeedPercent(motor, relPercent); /* set speed. This will care about the direction too */
|
|
}
|
|
|
|
void MOT_SetDirection(MOT_MotorDevice *motor, MOT_Direction dir) {
|
|
if (dir==MOT_DIR_FORWARD ) {
|
|
#if MOTOR_HAS_INVERT
|
|
motor->DirPutVal(motor->inverted?0:1);
|
|
#else
|
|
motor->DirPutVal(1);
|
|
#endif
|
|
if (motor->currSpeedPercent<0) {
|
|
motor->currSpeedPercent = -motor->currSpeedPercent;
|
|
}
|
|
} else if (dir==MOT_DIR_BACKWARD) {
|
|
#if MOTOR_HAS_INVERT
|
|
motor->DirPutVal(motor->inverted?1:0);
|
|
#else
|
|
motor->DirPutVal(0);
|
|
#endif
|
|
if (motor->currSpeedPercent>0) {
|
|
motor->currSpeedPercent = -motor->currSpeedPercent;
|
|
}
|
|
}
|
|
}
|
|
|
|
MOT_Direction MOT_GetDirection(MOT_MotorDevice *motor) {
|
|
if (motor->currSpeedPercent<0) {
|
|
return MOT_DIR_BACKWARD;
|
|
} else {
|
|
return MOT_DIR_FORWARD;
|
|
}
|
|
}
|
|
|
|
#if PL_CONFIG_USE_SHELL
|
|
static void MOT_PrintHelp(const McuShell_StdIOType *io) {
|
|
McuShell_SendHelpStr((unsigned char*)"motor", (unsigned char*)"Group of motor commands\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows motor help or status\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" on|off", (unsigned char*)"Enables or disables motor\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" (L|R) forward|backward", (unsigned char*)"Change motor direction\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" (L|R) duty <number>", (unsigned char*)"Change motor PWM (-100..+100)%\r\n", io->stdOut);
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
McuShell_SendHelpStr((unsigned char*)" (L|R) brake (on|off)", (unsigned char*)"Enable/disable brake for motor\r\n", io->stdOut);
|
|
#endif
|
|
}
|
|
|
|
static void MOT_PrintStatus(const McuShell_StdIOType *io) {
|
|
unsigned char buf[32];
|
|
|
|
McuShell_SendStatusStr((unsigned char*)"Motor", (unsigned char*)"Motor status\r\n", io->stdOut);
|
|
#ifdef MOTOR_HAS_INVERT
|
|
McuShell_SendStatusStr((unsigned char*)" inverted L", MOT_GetMotorHandle(MOT_MOTOR_LEFT)->inverted?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
|
|
McuShell_SendStatusStr((unsigned char*)" inverted R", MOT_GetMotorHandle(MOT_MOTOR_RIGHT)->inverted?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
|
|
#endif
|
|
McuShell_SendStatusStr((unsigned char*)" on/off", isMotorOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
|
|
McuShell_SendStatusStr((unsigned char*)" motor L", (unsigned char*)"", io->stdOut);
|
|
buf[0] = '\0';
|
|
McuUtility_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorL.currSpeedPercent, ' ', 4);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"% 0x");
|
|
McuUtility_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorL));
|
|
McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorL)==MOT_DIR_FORWARD?", fw":", bw"));
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorL)?", brake on":", brake off"));
|
|
#endif
|
|
McuShell_SendStr(buf, io->stdOut);
|
|
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((unsigned char*)" motor R", (unsigned char*)"", io->stdOut);
|
|
buf[0] = '\0';
|
|
McuUtility_Num16sToStrFormatted(buf, sizeof(buf), (int16_t)motorR.currSpeedPercent, ' ', 4);
|
|
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"% 0x");
|
|
McuUtility_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(&motorR));
|
|
McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetDirection(&motorR)==MOT_DIR_FORWARD?", fw":", bw"));
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
McuUtility_strcat(buf, sizeof(buf),(unsigned char*)(MOT_GetBrake(&motorR)?", brake on":", brake off"));
|
|
#endif
|
|
McuShell_SendStr(buf, io->stdOut);
|
|
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
|
|
|
|
#if PL_HAS_MOTOR_CURRENT_SENSE
|
|
WriteCurrent(&motorA, io);
|
|
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
|
|
WriteCurrent(&motorB, io);
|
|
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
|
|
#endif
|
|
}
|
|
|
|
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
uint8_t res = ERR_OK;
|
|
int32_t val;
|
|
const unsigned char *p;
|
|
|
|
if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"motor help")==0) {
|
|
MOT_PrintHelp(io);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"motor status")==0) {
|
|
MOT_PrintStatus(io);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor L forward")==0) {
|
|
MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor R forward")==0) {
|
|
MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor L backward")==0) {
|
|
MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor R backward")==0) {
|
|
MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
|
|
if (!isMotorOn) {
|
|
McuShell_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
} else {
|
|
p = cmd+sizeof("motor L duty");
|
|
if (McuUtility_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
|
|
MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
|
|
if (!isMotorOn) {
|
|
McuShell_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
} else {
|
|
p = cmd+sizeof("motor R duty");
|
|
if (McuUtility_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
|
|
MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
|
|
*handled = TRUE;
|
|
} else {
|
|
McuShell_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
|
|
res = ERR_FAILED;
|
|
}
|
|
}
|
|
} else if (McuUtility_strncmp((char*)cmd, (char*)"motor on", sizeof("motor on")-1)==0) {
|
|
isMotorOn = TRUE;
|
|
*handled = TRUE;
|
|
} else if (McuUtility_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 (McuUtility_strcmp((char*)cmd, (char*)"motor L brake on")==0) {
|
|
MOT_SetBrake(&motorL, TRUE);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor L brake off")==0) {
|
|
MOT_SetBrake(&motorL, FALSE);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor R brake on")==0) {
|
|
MOT_SetBrake(&motorR, TRUE);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((char*)cmd, (char*)"motor R brake off")==0) {
|
|
MOT_SetBrake(&motorR, FALSE);
|
|
*handled = TRUE;
|
|
#endif
|
|
}
|
|
return res;
|
|
}
|
|
#endif /* PL_CONFIG_USE_SHELL */
|
|
|
|
void MOT_Deinit(void) {
|
|
MOT_DirL = McuGPIO_DeinitGPIO(MOT_DirL);
|
|
MOT_DirR = McuGPIO_DeinitGPIO(MOT_DirR);
|
|
}
|
|
|
|
static void MOT_DirPinInit(void) {
|
|
McuGPIO_Config_t gpioConfig;
|
|
|
|
/* DIRL: PTC9
|
|
* DIRR: PTC8
|
|
*/
|
|
McuGPIO_GetDefaultConfig(&gpioConfig);
|
|
gpioConfig.hw.gpio = GPIOC;
|
|
gpioConfig.hw.port = PORTC;
|
|
gpioConfig.hw.pin = 9;
|
|
gpioConfig.isInput = false;
|
|
gpioConfig.isHighOnInit = true;
|
|
MOT_DirL = McuGPIO_InitGPIO(&gpioConfig);
|
|
gpioConfig.hw.pin = 8;
|
|
MOT_DirR = McuGPIO_InitGPIO(&gpioConfig);
|
|
}
|
|
|
|
void MOT_Init(void) {
|
|
MOT_DirPinInit();
|
|
#if PL_HAS_MOTOR_BRAKE
|
|
motorL.BrakePutVal = BrakeLPutVal;
|
|
motorR.BrakePutVal = BrakeRPutVal;
|
|
#endif
|
|
#if MOTOR_HAS_INVERT
|
|
motorL.inverted = FALSE;
|
|
motorR.inverted = FALSE;
|
|
#endif
|
|
motorL.DirPutVal = DirLPutVal;
|
|
motorR.DirPutVal = DirRPutVal;
|
|
motorL.SetRatio16 = PWMLSetRatio16;
|
|
motorR.SetRatio16 = PWMRSetRatio16;
|
|
MOT_SetSpeedPercent(&motorL, 0);
|
|
MOT_SetSpeedPercent(&motorR, 0);
|
|
TMR_MotorPWMStart();
|
|
}
|
|
#endif /* PL_CONFIG_USE_MOTORS */
|
|
|