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@1:681973914ddd, 2014-10-17 (annotated)
- Committer:
- JalilChavez
- Date:
- Fri Oct 17 21:48:31 2014 +0000
- Revision:
- 1:681973914ddd
- Parent:
- 0:4fc76af15d0e
- Corrected differential speed algorithm
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 | 1:681973914ddd | 39 | MTRCTL__xPwm[u8MotorIndex].write(((float)u16InPwr/MAX_POWER)*((float)MTRCTL__MaxSpeed/MAX_POWER)); |
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 | } |