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@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?

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