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
Diff: mtrctl.cpp
- Revision:
- 0:4fc76af15d0e
- Child:
- 1:681973914ddd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mtrctl.cpp Tue Oct 14 03:55:04 2014 +0000 @@ -0,0 +1,84 @@ +#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)*(MTRCTL__MaxSpeed)); + } + 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 ); +} \ No newline at end of file