Andrew's git - arduino-weather.git/blob - libraries/Mitov/Mitov_Steering

   1////////////////////////////////////////////////////////////////////////////////
   2//                                                                            //
   3//     This software is supplied under the terms of a license agreement or    //
   4//     nondisclosure agreement with Mitov Software and may not be copied      //
   5//     or disclosed except in accordance with the terms of that agreement.    //
   6//         Copyright(c) 2002-2016 Mitov Software. All Rights Reserved.        //
   7//                                                                            //
   8////////////////////////////////////////////////////////////////////////////////
   9
  10#ifndef _MITOV_STEERING_DIFFERENTIAL_h
  11#define _MITOV_STEERING_DIFFERENTIAL_h
  12
  13#include <Mitov.h>
  14
  15namespace Mitov
  16{
  17        class SteeringDifferential : public OpenWire::Component
  18        {
  19                typedef OpenWire::Component inherited;
  20
  21        public:
  22                OpenWire::SinkPin       SteeringInputPins[ 2 ];
  23                OpenWire::SourcePin     MotorsOutputPins[ 2 ];
  24
  25        protected:
  26                float FDirection = 0.5f;
  27                float FSpeed = 0.5f;
  28
  29        protected:
  30                void UpdateMotors()
  31                {
  32                        float   ADirOffset = FDirection - 0.5;
  33
  34                        float   ALeftMotor = FSpeed + ADirOffset;
  35                        float   ARightMotor = FSpeed - ADirOffset;
  36
  37                        if( ALeftMotor > 1.0 )
  38                        {
  39                                ARightMotor -= ( ALeftMotor - 1 );
  40                                ALeftMotor = 1.0;
  41                        }
  42
  43                        else if( ALeftMotor < 0.0 )
  44                        {
  45                                ARightMotor -= ALeftMotor;
  46                                ALeftMotor = 0.0;
  47                        }
  48
  49                        else if( ARightMotor > 1.0 )
  50                        {
  51                                ALeftMotor -= ( ARightMotor - 1 );
  52                                ARightMotor = 1.0;
  53                        }
  54
  55                        else if( ARightMotor < 0.0 )
  56                        {
  57                                ALeftMotor -= ARightMotor;
  58                                ARightMotor = 0.0;
  59                        }
  60
  61                        MotorsOutputPins[ 0 ].Notify( &ALeftMotor );
  62                        MotorsOutputPins[ 1 ].Notify( &ARightMotor );
  63                }
  64
  65        protected:
  66                void DoDirectionReceive( void *_Data )
  67                {
  68                        float AValue = constrain( *(float *)_Data, 0, 1 );
  69                        if( FDirection == AValue )
  70                                return;
  71
  72                        FDirection = AValue;
  73                        UpdateMotors();
  74                }
  75
  76                void DoSpeedReceive( void *_Data )
  77                {
  78                        float AValue = constrain( *(float *)_Data, 0, 1 );
  79                        if( FSpeed == AValue )
  80                                return;
  81
  82                        FSpeed = AValue;
  83                        UpdateMotors();
  84                }
  85
  86        public:
  87                SteeringDifferential()
  88                {
  89                        SteeringInputPins[ 0 ].SetCallback( this, (OpenWire::TOnPinReceive)&SteeringDifferential::DoDirectionReceive );
  90                        SteeringInputPins[ 1 ].SetCallback( this, (OpenWire::TOnPinReceive)&SteeringDifferential::DoSpeedReceive );
  91                }
  92
  93        };
  94//---------------------------------------------------------------------------
  95}
  96
  97#endif