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@0:4fc76af15d0e, 2014-10-14 (annotated)
- Committer:
- JalilChavez
- Date:
- Tue Oct 14 03:55:04 2014 +0000
- Revision:
- 0:4fc76af15d0e
- Child:
- 1:681973914ddd
controller of DC motors, this module modifies the duty cycle provided to each motor and also handles the difference of speed necessary to help the car during curved paths.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JalilChavez | 0:4fc76af15d0e | 1 | #include "mtrctl.h" |
JalilChavez | 0:4fc76af15d0e | 2 | |
JalilChavez | 0:4fc76af15d0e | 3 | /********************************************************************************** |
JalilChavez | 0:4fc76af15d0e | 4 | * Object declaration |
JalilChavez | 0:4fc76af15d0e | 5 | ***********************************************************************************/ |
JalilChavez | 0:4fc76af15d0e | 6 | PwmOut MTRCTL__xPwm[nNoOfMotors] = { D5, D7 }; |
JalilChavez | 0:4fc76af15d0e | 7 | DigitalOut MTRCTL__xEnable[nNoOfMotors] = { D4, D6 }; |
JalilChavez | 0:4fc76af15d0e | 8 | |
JalilChavez | 0:4fc76af15d0e | 9 | /********************************************************************************** |
JalilChavez | 0:4fc76af15d0e | 10 | * Local variable declaration |
JalilChavez | 0:4fc76af15d0e | 11 | ***********************************************************************************/ |
JalilChavez | 0:4fc76af15d0e | 12 | static uint16_t MTRCTL__MaxSpeed = MAX_POWER; |
JalilChavez | 0:4fc76af15d0e | 13 | |
JalilChavez | 0:4fc76af15d0e | 14 | void MTRCTL_vInit(void) |
JalilChavez | 0:4fc76af15d0e | 15 | { |
JalilChavez | 0:4fc76af15d0e | 16 | uint8_t u8MotorIndex = 0; |
JalilChavez | 0:4fc76af15d0e | 17 | /* Initial pwm frequency */ |
JalilChavez | 0:4fc76af15d0e | 18 | for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ ) |
JalilChavez | 0:4fc76af15d0e | 19 | { |
JalilChavez | 0:4fc76af15d0e | 20 | MTRCTL__xPwm[u8MotorIndex].period_us(OP_PERIOD); |
JalilChavez | 0:4fc76af15d0e | 21 | MTRCTL__xEnable[u8MotorIndex] = 1; |
JalilChavez | 0:4fc76af15d0e | 22 | } |
JalilChavez | 0:4fc76af15d0e | 23 | } |
JalilChavez | 0:4fc76af15d0e | 24 | |
JalilChavez | 0:4fc76af15d0e | 25 | void MTRCTL_vSetMaxSpeed( uint16_t u16MaxSpeed ) |
JalilChavez | 0:4fc76af15d0e | 26 | { |
JalilChavez | 0:4fc76af15d0e | 27 | MTRCTL__MaxSpeed = u16MaxSpeed; |
JalilChavez | 0:4fc76af15d0e | 28 | } |
JalilChavez | 0:4fc76af15d0e | 29 | |
JalilChavez | 0:4fc76af15d0e | 30 | void MTRCTL_vSetMotorPower( uint16_t u16InPwr, MTRCTL_tenMotors enMotor ) |
JalilChavez | 0:4fc76af15d0e | 31 | { |
JalilChavez | 0:4fc76af15d0e | 32 | uint8_t u8MotorIndex = 0; |
JalilChavez | 0:4fc76af15d0e | 33 | for( u8MotorIndex = 0; u8MotorIndex < (uint8_t)(nNoOfMotors); u8MotorIndex++ ) |
JalilChavez | 0:4fc76af15d0e | 34 | { |
JalilChavez | 0:4fc76af15d0e | 35 | if( u8MotorIndex == (uint8_t)(enMotor) ) |
JalilChavez | 0:4fc76af15d0e | 36 | { |
JalilChavez | 0:4fc76af15d0e | 37 | if( u16InPwr < MAX_POWER ) |
JalilChavez | 0:4fc76af15d0e | 38 | { |
JalilChavez | 0:4fc76af15d0e | 39 | MTRCTL__xPwm[u8MotorIndex].write(((float)u16InPwr/MAX_POWER)*(MTRCTL__MaxSpeed)); |
JalilChavez | 0:4fc76af15d0e | 40 | } |
JalilChavez | 0:4fc76af15d0e | 41 | else |
JalilChavez | 0:4fc76af15d0e | 42 | { |
JalilChavez | 0:4fc76af15d0e | 43 | MTRCTL__xPwm[u8MotorIndex].write(1.0); |
JalilChavez | 0:4fc76af15d0e | 44 | } |
JalilChavez | 0:4fc76af15d0e | 45 | } |
JalilChavez | 0:4fc76af15d0e | 46 | else |
JalilChavez | 0:4fc76af15d0e | 47 | { |
JalilChavez | 0:4fc76af15d0e | 48 | /* Motor not found, do nothing */ |
JalilChavez | 0:4fc76af15d0e | 49 | } |
JalilChavez | 0:4fc76af15d0e | 50 | } |
JalilChavez | 0:4fc76af15d0e | 51 | } |
JalilChavez | 0:4fc76af15d0e | 52 | |
JalilChavez | 0:4fc76af15d0e | 53 | void MTRCTL_vSetSystemSpeed( uint16_t u16Speed, uint8_t u8Direction ) |
JalilChavez | 0:4fc76af15d0e | 54 | { |
JalilChavez | 0:4fc76af15d0e | 55 | uint16_t u16RightCompensation = 0; |
JalilChavez | 0:4fc76af15d0e | 56 | uint16_t u16LeftCompensation = 0; |
JalilChavez | 0:4fc76af15d0e | 57 | uint16_t u16CompError = 0; |
JalilChavez | 0:4fc76af15d0e | 58 | uint16_t u16FinalSpeed = 0; |
JalilChavez | 0:4fc76af15d0e | 59 | |
JalilChavez | 0:4fc76af15d0e | 60 | u16RightCompensation = (uint16_t)( SCALE_DIR2SPEED * u8Direction ); |
JalilChavez | 0:4fc76af15d0e | 61 | u16LeftCompensation = (uint16_t)( SCALE_DIR2SPEED * (MAX_DIRECTION_VAL - u8Direction) ); |
JalilChavez | 0:4fc76af15d0e | 62 | |
JalilChavez | 0:4fc76af15d0e | 63 | if( u16RightCompensation > u16LeftCompensation ) |
JalilChavez | 0:4fc76af15d0e | 64 | { |
JalilChavez | 0:4fc76af15d0e | 65 | u16CompError = SCALE_DIR2SPEED * ( u16RightCompensation - u16LeftCompensation ); |
JalilChavez | 0:4fc76af15d0e | 66 | } |
JalilChavez | 0:4fc76af15d0e | 67 | else |
JalilChavez | 0:4fc76af15d0e | 68 | { |
JalilChavez | 0:4fc76af15d0e | 69 | u16CompError = SCALE_DIR2SPEED * ( u16LeftCompensation - u16RightCompensation ); |
JalilChavez | 0:4fc76af15d0e | 70 | } |
JalilChavez | 0:4fc76af15d0e | 71 | |
JalilChavez | 0:4fc76af15d0e | 72 | if( u16Speed > u16CompError ) |
JalilChavez | 0:4fc76af15d0e | 73 | { |
JalilChavez | 0:4fc76af15d0e | 74 | u16FinalSpeed = u16Speed - u16CompError; |
JalilChavez | 0:4fc76af15d0e | 75 | } |
JalilChavez | 0:4fc76af15d0e | 76 | else |
JalilChavez | 0:4fc76af15d0e | 77 | { |
JalilChavez | 0:4fc76af15d0e | 78 | u16FinalSpeed = MIN_POWER; |
JalilChavez | 0:4fc76af15d0e | 79 | } |
JalilChavez | 0:4fc76af15d0e | 80 | |
JalilChavez | 0:4fc76af15d0e | 81 | MTRCTL_vSetMaxSpeed(u16FinalSpeed); |
JalilChavez | 0:4fc76af15d0e | 82 | MTRCTL_vSetMotorPower( u16RightCompensation , nRightMotor ); |
JalilChavez | 0:4fc76af15d0e | 83 | MTRCTL_vSetMotorPower( u16LeftCompensation , nLeftMotor ); |
JalilChavez | 0:4fc76af15d0e | 84 | } |