MTRCTL - Sets initial frequency of DC motors. - Controls motor speed according to IC specifications. - Handles motors in order to help the car during curved paths.
Dependents: frdm_MasterVehicle
mtrctl.cpp
- Committer:
- JalilChavez
- Date:
- 2014-10-17
- Revision:
- 1:681973914ddd
- Parent:
- 0:4fc76af15d0e
File content as of revision 1:681973914ddd:
#include "mtrctl.h" /********************************************************************************** * Object declaration ***********************************************************************************/ PwmOut MTRCTL__xPwm[nNoOfMotors] = { D5, D7 }; DigitalOut MTRCTL__xEnable[nNoOfMotors] = { D4, D6 }; /********************************************************************************** * Local variable declaration ***********************************************************************************/ static uint16_t MTRCTL__MaxSpeed = MAX_POWER; void MTRCTL_vInit(void) { uint8_t u8MotorIndex = 0; /* Initial pwm frequency */ for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ ) { MTRCTL__xPwm[u8MotorIndex].period_us(OP_PERIOD); MTRCTL__xEnable[u8MotorIndex] = 1; } } void MTRCTL_vSetMaxSpeed( uint16_t u16MaxSpeed ) { MTRCTL__MaxSpeed = u16MaxSpeed; } void MTRCTL_vSetMotorPower( uint16_t u16InPwr, MTRCTL_tenMotors enMotor ) { uint8_t u8MotorIndex = 0; for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ ) { if( u8MotorIndex == (uint8_t)(enMotor) ) { if( u16InPwr < MAX_POWER ) { MTRCTL__xPwm[u8MotorIndex].write(((float)u16InPwr/MAX_POWER)*((float)MTRCTL__MaxSpeed/MAX_POWER)); } else { MTRCTL__xPwm[u8MotorIndex].write(1.0); } } else { /* Motor not found, do nothing */ } } } void MTRCTL_vSetSystemSpeed( uint16_t u16Speed, uint8_t u8Direction ) { uint16_t u16RightCompensation = 0; uint16_t u16LeftCompensation = 0; uint16_t u16CompError = 0; uint16_t u16FinalSpeed = 0; u16RightCompensation = (uint16_t)( SCALE_DIR2SPEED * u8Direction ); u16LeftCompensation = (uint16_t)( SCALE_DIR2SPEED * (MAX_DIRECTION_VAL - u8Direction) ); if( u16RightCompensation > u16LeftCompensation ) { u16CompError = SCALE_DIR2SPEED * ( u16RightCompensation - u16LeftCompensation ); } else { u16CompError = SCALE_DIR2SPEED * ( u16LeftCompensation - u16RightCompensation ); } if( u16Speed > u16CompError ) { u16FinalSpeed = u16Speed - u16CompError; } else { u16FinalSpeed = MIN_POWER; } MTRCTL_vSetMaxSpeed(u16FinalSpeed); MTRCTL_vSetMotorPower( u16RightCompensation , nRightMotor ); MTRCTL_vSetMotorPower( u16LeftCompensation , nLeftMotor ); }