Instrumentation / MTRCTL

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?

UserRevisionLine numberNew 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 }