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